// --------------------------------------------------------------------------------------------------------------------
// <copyright file="NavigationManifest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections;
    using System.Collections.Generic;
    using System.IO;

    using Microsoft.Kinect;    
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.EV4.Runtime;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Navigation.Runtime.GroundTruthExtraction;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Navigation.Runtime.Motion;
    using Microsoft.Robotics.Navigation.Runtime.Perception;
    using Microsoft.Robotics.Navigation.Visualization;
    using Microsoft.Robotics.Navigation.Visualization.GroundTruthExtraction;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Runtime;
    using Microsoft.Robotics.Vision.Runtime.Cameras;
    using Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction;
    using Microsoft.Robotics.Visualization;

    using Test.Robotics.Hardware.EV4;
    using Test.Robotics.Interaction;
    using Test.Robotics.Metrics;
    using Test.Robotics.Navigation;
    using Test.Robotics.Runtime;
    using Test.Robotics.Vision;
    
    /// <summary>
    /// Provides methods for creating manifests in a type-checked way.
    /// </summary>
    public static class NavigationManifest
    {
        /// <summary>
        /// The distance cutoff for depth observations
        /// </summary>
        public const double DepthCutoff = DepthImageConstants.MaximumRange;

        /// <summary>
        /// The default rate for updating visualizations (this throttles the amount of processing done for visualization purposes)
        /// </summary>
        public const int DefaultVisualizationUpdateInterval = 100;

        /// <summary>
        /// Adds the HDP agent visualizer to the pipeline
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="views">The list to add views to.</param>
        /// <param name="hdp">The horizontal depth profile producer</param>
        /// <param name="mapResolution">The resolution in meters per pixel</param>
        /// <param name="depthCutoff">The distance cutoff for depth observations</param>
        /// <typeparam name="THorizontalDepthProfile">The type of hdp to consume</typeparam>
        public static void AddHorizontalDepthProfileModelView<THorizontalDepthProfile>(
            Manifest manifest,
            List<ViewDefinition> views,
            IProducer<SimpleAgentMessage<THorizontalDepthProfile>> hdp,
            double mapResolution,
            double depthCutoff)
            where THorizontalDepthProfile : IHorizontalDepthProfile
        {
            HorizontalDepthProfileModelAgent<THorizontalDepthProfile> hdpModel = new HorizontalDepthProfileModelAgent<THorizontalDepthProfile>(
                name: "KinectHorizontalDepthProfileModelAgent",
                producer: hdp,
                maxUpdateIntervalMilliseconds: DefaultVisualizationUpdateInterval,
                maximumDepth: depthCutoff,
                resolutionInMetersPerPixel: mapResolution);
            manifest.Agents.Add(hdpModel);

            ViewDefinition hdpView = new ViewDefinition(
                name: "KinectHorizontalDepthProfile",
                viewType: typeof(HorizontalDepthProfileView),
                modelAgent: hdpModel);
            views.Add(hdpView);
        }

        /// <summary>
        /// Adds the HDP agent to the pipeline
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="views">The list to add views to.</param>
        /// <param name="kinectProcessor">The kinect processor agent that the hdp consumes</param>
        /// <param name="robotConfigFolder">The path to the file containing the intrinsic parameters of the depth camera</param>
        /// <returns>The hdp producer interface</returns>
        public static KinectHorizontalDepthProfileAgent AddKinectHorizontalDepthProfile(
            Manifest manifest,
            List<ViewDefinition> views,
            IProducer<DepthCameraAgentMessage<KinectDepthCameraParameters>> kinectProcessor,
            string robotConfigFolder)
        {
            KinectHorizontalDepthProfileAgent hdp = new KinectHorizontalDepthProfileAgent(
               name: "KinectHorizontalDepthProfileAgent",
               intrinsicParametersFolder: robotConfigFolder,
               producer: kinectProcessor);
            manifest.Agents.Add(hdp);

            NavigationManifest.AddHorizontalDepthProfileModelView(manifest, views, hdp, GlobalMap.DefaultDistanceResolution, DepthCutoff);
            return hdp;
        }

        /// <summary>
        /// Adds a remoting proxy to broadcast HDP messages over the network.
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="hdp">The agent to network-enable.</param>
        /// <param name="targetNameOrAddress">The name or IP address of the machine to broadcast to (e.g. a desktop machine)</param>
        /// <returns>A remoting proxy</returns>
        public static ProxyDefinition AddKinectHorizontalDepthProfileNetworkEnabledProxy(
            Manifest manifest,
            IProducer<SimpleAgentMessage<KinectHorizontalDepthProfile>> hdp,
            string targetNameOrAddress)
        {
            ProxyDefinition hdpProxy = ProxyDefinition.CreateNetworkEnabledProxy(
                name: hdp.Name + "Proxy",
                producer: hdp.Name,
                address: targetNameOrAddress,
                publishContracts: new[] { typeof(SimpleAgentMessage<KinectHorizontalDepthProfile>) },
                publishNetworkPortNumber: (int)RemotingPorts.EV4DepthProfilePublish,
                controlContracts: null,
                controlNetworkPortNumber: 0);
            manifest.NetworkEnabledAgentProxies.Add(hdpProxy);

            return hdpProxy;
        }

        /// <summary>
        /// Adds a desktop proxy for a remote HDP agent running on the robot.
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="views">The set of views to add to</param>
        /// <param name="robotNameOrAddress">The name or IP address of the robot</param>
        /// <returns>The hdp proxy</returns>
        public static IProducer<SimpleAgentMessage<KinectHorizontalDepthProfile>> AddKinectHorizontalDepthProfileRemoteProxy(
            Manifest manifest,
            List<ViewDefinition> views,
            string robotNameOrAddress)
        {
            // now create a proxy with the same name and add it to the manifest
            ProxyDefinition remoteProxy = ProxyDefinition.CreateRemoteProxy(
                name: "HorizontalDepthProfileAgent",
                address: robotNameOrAddress,
                publishContracts: new[] { typeof(SimpleAgentMessage<KinectHorizontalDepthProfile>) },
                publishNetworkPortNumber: (int)RemotingPorts.EV4DepthProfilePublish,
                controlContracts: null,
                controlNetworkPortNumber: 0);
            manifest.RemoteAgentProxies.Add(remoteProxy);

            return remoteProxy.AsProducer<SimpleAgentMessage<KinectHorizontalDepthProfile>>();
        }

        /// <summary>
        /// Adds the DifferentialDriveAgent agent to the pipeline
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="views">The list to add views to.</param>
        /// <param name="heartbeatAgent">The differential drive producer</param>
        /// <param name="robotConfigFolder">Robot configuration folder</param>
        /// <returns>The drive feedback producer interface</returns>
        public static DifferentialDriveAgent AddDifferentialDrive(Manifest manifest, List<ViewDefinition> views, IProducer<DifferentialDriveMessage> heartbeatAgent, string robotConfigFolder)
        {
            DifferentialDriveAgent driveFeedback = new DifferentialDriveAgent(
               name: "DifferentialDriveAgent",
               producer: heartbeatAgent,
               robotConfigFolder: robotConfigFolder);

            manifest.Agents.Add(driveFeedback);
            return driveFeedback;
        }

        /// <summary>
        /// Adds the SLAM mapper drive agent to the pipeline.
        /// </summary>
        /// <param name="manifest">The manifest to add to</param>
        /// <param name="views">The view collection to add any visualizations to</param>
        /// <param name="robotConfigFolder">Robot config folder</param>
        /// <param name="gamepadAgent">The gamepad input agent</param>
        /// <param name="heartbeatAgent">Heart beat agent</param>
        /// <param name="kinectAgent">Kinect agent</param>
        /// <param name="driveFeedbackAgent">Drive feedback agent</param>
        /// <returns>The SLAM mapper drive agent</returns>
        public static SlamMapperDriveAgent AddSlamMapperDrive(Manifest manifest, List<ViewDefinition> views, string robotConfigFolder, IProducer<GamePadAgentMessage> gamepadAgent, HeartbeatAgent heartbeatAgent, IProducer<DepthCameraAgentMessage<KinectDepthCameraParameters>> kinectAgent, IProducer<SimpleAgentMessage<DriveFeedbackState>> driveFeedbackAgent)
        {
            SlamMapperDriveAgent drive = new SlamMapperDriveAgent(
                name: "SlamMapperDriveAgent",
                gamePadProducer: gamepadAgent.Name,
                heartbeatAgentName: heartbeatAgent.Name,
                kinectAgentName: kinectAgent.Name,
                driveFeedbackAgentName: driveFeedbackAgent.Name,
                stopForCaptureIntervalInMeters: 7.5,
                turretSweepStepsInRadians: 1.05,
                turretHoldPositionInMilliseconds: 1000,
                turretSweepRangeInRadiansStart: -3.0,
                turretSweepRangeInRadiansEnd: 3.0,
                turretDefaultHeading: 0.0,
                driveSpeedMax: 0.25,
                driveRotationMax: 0.4,
                jointMessageProducer: heartbeatAgent.Name,
                considerTurretStableIfRotatingSlowerThanThis: 0.002,
                robotConfigFolder: robotConfigFolder,
                desiredTurretPanRotationSpeed: 3);
            manifest.Agents.Add(drive);

            return drive;
        }

        /// <summary>
        /// Adds the turret scanner agent to the pipeline.
        /// </summary>
        /// <param name="manifest">The manifest to add to</param>
        /// <param name="views">The view collection to add any visualizations to</param>
        /// <param name="robotConfigFolder">Robot config folder</param>
        /// <param name="heartbeatAgent">Heart beat agent</param>
        /// <param name="kinectRawAgent">Raw Kinect agent</param>
        /// <returns>The turret scanner agent</returns>
        public static TurretScannerAgent AddTurretScanner(Manifest manifest, List<ViewDefinition> views, string robotConfigFolder, HeartbeatAgent heartbeatAgent, IProducer<DepthCameraAgentMessage<KinectDepthCameraParameters>> kinectRawAgent)
        {
            TurretScannerAgent turretScanner = new TurretScannerAgent(
                name: "TurretScannerAgent",
                heartbeatAgentName: heartbeatAgent.Name,
                kinectAgentName: kinectRawAgent.Name,
                turretSweepStepsInRadians: 30 * MathConstants.Degrees2Radians,
                turretHoldPositionInMilliseconds: 500,
                turretSweepRangeInRadiansStart: -3.0,
                turretSweepRangeInRadiansEnd: 3.0,
                turretDefaultHeading: 0.0,
                considerTurretStableIfRotatingSlowerThanThis: 0.004,
                robotConfigFolder: robotConfigFolder,
                desiredTurretPanRotationSpeed: 1);

            manifest.Agents.Add(turretScanner);

            return turretScanner;
        }

        /// <summary>
        /// Creates a manifest for running path tracking on the robot.
        /// </summary>
        /// <param name="depthFormat">Depth image format.</param>
        /// <param name="robotConfigFolder"> Ev4 calibration folder path</param>
        /// <param name="environmentConfigFolder">Environment config folder path</param>
        /// <returns>A manifest to run path tracking on the robot.</returns>
        public static Manifest CreateLocalSuggestionDriveTestingManifest(DepthImageFormat depthFormat, string robotConfigFolder, string environmentConfigFolder)
        {
            Manifest manifest = new Manifest();
            var views = new List<ViewDefinition>();

            HeartbeatAgent heartbeat = EV4Manifest.AddEV4HeartbeatAgent(manifest, views, robotConfigFolder);
            manifest.ActiveAgents.Add(heartbeat.Name);

            // DifferencialDriveTracking (with remote game pad override [gamePad can be real (requires x86 on source machine) or emulated])
            IProducer<GamePadAgentMessage> gamePadAgent = InteractionManifest.AddGamePadAgent(manifest, views);
            manifest.ActiveAgents.Add(gamePadAgent.Name);

            // Kinect
            KinectAgent kinectRaw = VisionManifest.AddEV4Kinect(manifest, views, depthFormat, true);

            // Joystick
            IProducer<MobileBaseVelocityReferenceMessage> xboxControllerDriveAgent = NavigationManifest.AddXboxControllerDriveAgent(manifest, gamePadAgent);
            manifest.ActiveAgents.Add(xboxControllerDriveAgent.Name);

            // local suggestion drive (obstacle avoidance)
            Ev4LocalSuggestionDriveAgent localSuggestionDriveAgent = PathPlanningAndTrackingManifest.AddEv4LocalSuggestionDrive(manifest, views, xboxControllerDriveAgent, kinectRaw, heartbeat);
            manifest.ActiveAgents.Add(localSuggestionDriveAgent.Name);

            // Driving 
            DifferentialDriveTrackingAgent tracking = NavigationManifest.AddDifferentialDriveTracking(manifest, views, localSuggestionDriveAgent, heartbeat);
            manifest.ActiveAgents.Add(tracking.Name);

            return manifest;
        }

        /// <summary>
        /// Creates simple joystick drive agent
        /// </summary>
        /// <param name="manifest">Manifest to add to</param>
        /// <param name="gamePadAgent">Gamepad message producer</param>
        /// <returns>GamePad drive controller agent</returns>
        public static GamepadDriveAgent AddXboxControllerDriveAgent(Manifest manifest, IProducer<GamePadAgentMessage> gamePadAgent) 
        {
            GamepadDriveAgent joystickDriveAgent = new GamepadDriveAgent(
                name: "XboxControllerDriveAgent",
                gamePadProducer: gamePadAgent.Name,
                driveSpeedMax: 0.5,
                driveRotationMax: 0.5);

            manifest.Agents.Add(joystickDriveAgent);

            return joystickDriveAgent;
        }

        /// <summary>
        /// Adds the differential drive tracking agent (which converts published drive commands into drive control messages) to the pipeline.
        /// </summary>
        /// <param name="manifest">The manifest to add to</param>
        /// <param name="views">The view collection to add any visualizations to</param>
        /// <param name="driveAgent">The agent that publishes drive commands</param>
        /// <param name="driveControlAgent">The heartbeat agent that accepts drive control messages</param>
        /// <returns>The drive tracking agent</returns>
        public static DifferentialDriveTrackingAgent AddDifferentialDriveTracking(Manifest manifest, List<ViewDefinition> views, IProducer<MobileBaseVelocityReferenceMessage> driveAgent, IControllable<DifferentialDriveSetVelocityMessage> driveControlAgent)
        {
            DifferentialDriveTrackingAgent tracking = new DifferentialDriveTrackingAgent(
                name: "DifferentialDriveTrackingAgent",
                mobileBaseVelocityReferenceProducer: driveAgent,
                heartbeatAgent: driveControlAgent,
                wheelBase: 0.305,
                maxWheelSpeed: 1);
            manifest.Agents.Add(tracking);

            return tracking;
        }

        /// <summary>
        /// Adds the differential drive tracking agent (which converts published drive commands into drive control messages) to the pipeline
        /// and allows a gamepad to override those drive control messages
        /// </summary>
        /// <param name="manifest">The manifest to add to</param>
        /// <param name="views">The view collection to add any visualizations to</param>
        /// <param name="driveAgent">The agent that publishes drive commands</param>
        /// <param name="driveControlAgent">The heartbeat agent that accepts drive control messages</param>
        /// <param name="gamePadAgent">The game pad agent</param>
        /// <returns>The drive tracking agent</returns>
        public static DifferentialDriveTrackingAgent AddDifferentialDriveTrackingWithGamePadOverride(Manifest manifest, List<ViewDefinition> views, IProducer<MobileBaseVelocityReferenceMessage> driveAgent, IControllable<DifferentialDriveSetVelocityMessage> driveControlAgent, IProducer<GamePadAgentMessage> gamePadAgent)
        {
            VelocityOverrideAgent overrideAgent = new VelocityOverrideAgent(
                name: "VelocityOverrideAgent",
                gamePadProducer: gamePadAgent,
                velocityProducer: driveAgent,
                maxSpeedInMetersPerSecond: 0.5,
                maximumTurnDurationInMs: 5500,
                maxTurnRateInRadiansPerSecond: 0.8,
                minimumTurnDurationInMs: 1500);
            manifest.Agents.Add(overrideAgent);

            return AddDifferentialDriveTracking(manifest, views, overrideAgent, driveControlAgent);
        }

        /// <summary>
        /// Adds the SLAM mapper drive agent and its dependencies to the pipeline.
        /// </summary>
        /// <param name="manifest">The manifest to add to</param>
        /// <param name="views">The view collection to add any visualizations to</param>
        /// <param name="gamepadAgent">The gamepad input agent</param>
        /// <param name="driveControlAgent">The heartbeat agent that accepts drive control messages</param>
        /// <param name="heartbeatAgent">Heart beat agent</param>
        /// <param name="kinectAgent">Kinect agent</param>
        /// <param name="robotConfigFolder">Robot config folder</param>
        /// <returns>The drive tracking agent</returns>
        public static DifferentialDriveTrackingAgent AddSlamMapperDriveAndDependencies(
            Manifest manifest,
            List<ViewDefinition> views,
            IProducer<GamePadAgentMessage> gamepadAgent,
            IControllable<DifferentialDriveSetVelocityMessage> driveControlAgent,
            HeartbeatAgent heartbeatAgent,
            IProducer<DepthCameraAgentMessage<KinectDepthCameraParameters>> kinectAgent,
            string robotConfigFolder)
        {
            // drive remoting
            DifferentialDriveAgent diffDrive = NavigationManifest.AddDifferentialDrive(manifest, views, heartbeatAgent, robotConfigFolder);
            SlamMapperDriveAgent slamdriveAgent = NavigationManifest.AddSlamMapperDrive(manifest, views, robotConfigFolder, gamepadAgent, heartbeatAgent, kinectAgent, diffDrive);
            DifferentialDriveTrackingAgent tracking = NavigationManifest.AddDifferentialDriveTracking(manifest, views, slamdriveAgent, driveControlAgent);
            return tracking;
        }

        /// <summary>
        /// Adds an agent that corrects odometry info based on gyro data.
        /// </summary>
        /// <param name="manifest">The manifest to add the agent to</param>
        /// <param name="views">The views to add visualizations to</param>
        /// <param name="driveFeedback">The odometry producing agent</param>
        /// <param name="gyro">The gyro producing agent</param>
        /// <param name="robotConfigFolder">Robot configuration folder</param>
        /// <param name="requiredGyroMeasurementsForCalibration">Number of gyro samples to collect and calibrate before robot can move. Default is 400 samples, or around 16 seconds</param>        
        /// <returns>The virtual drive agent</returns>
        public static VirtualDriveFeedbackAgent AddVirtualDriveFeedback(
            Manifest manifest, 
            List<ViewDefinition> views, 
            IProducer<SimpleAgentMessage<DriveFeedbackState>> driveFeedback, 
            IProducer<GyroscopeMessage> gyro, 
            string robotConfigFolder, 
            int requiredGyroMeasurementsForCalibration = 400)
        {          
            VirtualDriveFeedbackAgent virtualDrive = new VirtualDriveFeedbackAgent(
                name: "VirtualDriveFeedbackAgent",
                odometryProducer: driveFeedback,                
                gyroProducer: gyro,
                robotConfigFolder: robotConfigFolder,
                requiredGyroMeasurementsForCalibration: requiredGyroMeasurementsForCalibration);

            manifest.Agents.Add(virtualDrive);
            return virtualDrive;
        }

        /// <summary>
        /// Adds a remoting proxy to broadcast drive feedback messages over the network.
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="driveFeedback">The agent to network-enable.</param>
        /// <param name="targetNameOrAddress">The name or IP address of the machine to broadcast to (e.g. a desktop machine)</param>
        /// <returns>A remoting proxy</returns>
        public static ProxyDefinition AddDriveFeedbackNetworkEnabledProxy(Manifest manifest, IProducer<SimpleAgentMessage<DriveFeedbackState>> driveFeedback, string targetNameOrAddress)
        {
            ProxyDefinition driveFeedbackProxy = ProxyDefinition.CreateNetworkEnabledProxy(
                name: driveFeedback.Name + "Proxy",
                producer: driveFeedback.Name,
                address: targetNameOrAddress,
                publishContracts: new[] { typeof(SimpleAgentMessage<DriveFeedbackState>) },
                publishNetworkPortNumber: (int)RemotingPorts.DriveFeedbackPublish,
                controlContracts: null,
                controlNetworkPortNumber: 0);
            manifest.NetworkEnabledAgentProxies.Add(driveFeedbackProxy);

            return driveFeedbackProxy;
        }

        /// <summary>
        /// Adds a desktop proxy for a remote drive feedback agent running on the robot.
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="views">The set of views to add to.</param>
        /// <param name="robotNameOrAddress">The name or IP address of the robot.</param>
        /// <returns>The drive feedback proxy</returns>
        public static IProducer<SimpleAgentMessage<DriveFeedbackState>> AddDriveFeedbackRemoteProxy(Manifest manifest, List<ViewDefinition> views, string robotNameOrAddress)
        {
            ProxyDefinition remoteProxy = ProxyDefinition.CreateRemoteProxy(
                name: "DriveFeedbackAgent",
                address: robotNameOrAddress,
                publishContracts: new[] { typeof(SimpleAgentMessage<DriveFeedbackState>) },
                publishNetworkPortNumber: (int)RemotingPorts.DriveFeedbackPublish,
                controlContracts: null,
                controlNetworkPortNumber: 0);
            manifest.RemoteAgentProxies.Add(remoteProxy);

            return remoteProxy.AsProducer<SimpleAgentMessage<DriveFeedbackState>>();
        }
             
        /// <summary>
        /// Adds positioning precision calculator to the pipeline
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="views">The list to add views to.</param>
        /// <param name="slamPose">Slam pose message producer</param>
        /// <param name="groundTruth">Ground truth producer</param>
        /// <param name="completion">Completion message producer</param>
        /// <param name="syncTolerance">Synchronization tolerance</param>
        /// <returns>Positioning precision calculator </returns>
        public static PositioningPrecisionCalculator AddPositioningPrecisionCalculator(
            Manifest manifest,
            List<ViewDefinition> views,
            IProducer<SLAM2DPoseMessage> slamPose,
            IProducer<GroundTruthMessage> groundTruth,
            IProducer<CompletionMessage> completion,
            int syncTolerance = PositioningPrecisionCalculator.DefaultSyncTolerance)
        {
            PositioningPrecisionCalculator calc = new PositioningPrecisionCalculator(
                "calculator",
                slamPose,
                groundTruth,
                completion,
                syncTolerance);
            manifest.Agents.Add(calc);
            return calc;
        }

        /// <summary>
        /// Adds Slam 2D to Pose 2D transformer agent to the pipeline
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="views">The list to add views to.</param>
        /// <param name="slam">Slam agent message producer</param>
        /// <returns>Slam 2D to Pose 2D transformer agent</returns>
        public static SLAM2DToPose2DTransformerAgent AddSlam2DToPose2DTransformerAgent(Manifest manifest, List<ViewDefinition> views, IProducer<SLAM2DAgentMessage> slam)
        {
            SLAM2DToPose2DTransformerAgent slam2DToPose2DTransformerAgent = new SLAM2DToPose2DTransformerAgent("Slam2DToPose2DTransformer", slam);
            manifest.Agents.Add(slam2DToPose2DTransformerAgent);
            return slam2DToPose2DTransformerAgent;
        }

        /// <summary>
        /// Adds the JointMessageInterpolatorAgent agent to the pipeline
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="views">The list to add views to.</param>
        /// <param name="jointMessageProducer">The joint message producer</param>
        /// <param name="kinectMessageProducer">The kinect message producer</param>
        /// <returns>The joint message interpolator agent which interpolates joint messages at kinect message timestamps</returns>
        public static JointMessageInterpolatorAgent AddJointMessageInterpolator(Manifest manifest, List<ViewDefinition> views, IProducer<JointMessage> jointMessageProducer, IProducer<DepthCameraAgentMessage<KinectDepthCameraParameters>> kinectMessageProducer)
        {
            JointMessageInterpolatorAgent jointInterpolatorAgent = new JointMessageInterpolatorAgent(
               name: "JointInterpolatorAgent",
               jointMessageProducer: jointMessageProducer.Name,
               kinectMessageProducer: kinectMessageProducer.Name);

            manifest.Agents.Add(jointInterpolatorAgent);
            return jointInterpolatorAgent;
        }

        /// <summary>
        /// Adds the GyroscopeMessageInterpolatorAgent agent to the pipeline
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="views">The list to add views to.</param>
        /// <param name="gyroscopeMessageProducer">The gyroscope message producer</param>
        /// <param name="kinectMessageProducer">The kinect message producer</param>
        /// <returns>The joint message interpolator agent which interpolates gyroscope messages at kinect message timestamps</returns>
        public static GyroscopeMessageInterpolatorAgent AddGyroscopeMessageInterpolator(Manifest manifest, List<ViewDefinition> views, IProducer<GyroscopeMessage> gyroscopeMessageProducer, IProducer<DepthCameraAgentMessage<KinectDepthCameraParameters>> kinectMessageProducer)
        {
            GyroscopeMessageInterpolatorAgent gyroscopeInterpolatorAgent = new GyroscopeMessageInterpolatorAgent(
               name: "GyroInterpolatorAgent",
               kinectMessageProducer: kinectMessageProducer.Name,
               gyroscopeMessageProducer: gyroscopeMessageProducer.Name);

            manifest.Agents.Add(gyroscopeInterpolatorAgent);
            return gyroscopeInterpolatorAgent;
        }

        /// <summary>
        /// Adds the DifferentialDriveMessageInterpolatorAgent agent to the pipeline
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="views">The list to add views to.</param>
        /// <param name="differentialDriveMessageProducer">The differentialDrive message producer</param>
        /// <param name="kinectMessageProducer">The kinect message producer</param>
        /// <returns>The differentialDrive message interpolator agent which interpolates differentialDrive messages at kinect message timestamps</returns>
        public static DifferentialDriveMessageInterpolatorAgent AddDifferentialDriveMessageInterpolator(Manifest manifest, List<ViewDefinition> views, IProducer<DifferentialDriveMessage> differentialDriveMessageProducer, IProducer<DepthCameraAgentMessage<KinectDepthCameraParameters>> kinectMessageProducer)
        {
            DifferentialDriveMessageInterpolatorAgent differentialDriveInterpolatorAgent = new DifferentialDriveMessageInterpolatorAgent(
               name: "DiffDriveInterpolatorAgent",
               kinectMessageProducer: kinectMessageProducer.Name,
               differentialDriveMessageProducer: differentialDriveMessageProducer.Name);

            manifest.Agents.Add(differentialDriveInterpolatorAgent);
            return differentialDriveInterpolatorAgent;
        }

        /// <summary>
        /// Adds the PoseFeedbackMessageInterpolatorAgent agent to the pipeline
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="views">The list to add views to.</param>
        /// <param name="poseFeedbackMessageProducer">The poseFeedback message producer</param>
        /// <param name="hdpMessageProducer">The hdp message producer</param>
        /// <returns>The poseFeedback message interpolator agent which interpolates poseFeedback messages at kinect message timestamps</returns>
        public static PoseFeedbackMessageInterpolatorAgent AddPoseFeedbackMessageInterpolator(Manifest manifest, List<ViewDefinition> views, IProducer<SimpleAgentMessage<PoseFeedbackState>> poseFeedbackMessageProducer, IProducer<SimpleAgentMessage<LaserHorizontalDepthProfile>> hdpMessageProducer)
        {
            PoseFeedbackMessageInterpolatorAgent poseFeedbackInterpolatorAgent = new PoseFeedbackMessageInterpolatorAgent(
               name: "DiffDriveInterpolatorAgent",
               hdpMessageProducer: hdpMessageProducer.Name,
               poseFeedbackMessageProducer: poseFeedbackMessageProducer.Name);

            manifest.Agents.Add(poseFeedbackInterpolatorAgent);
            return poseFeedbackInterpolatorAgent;
        }

        /// <summary>
        /// Adds the KinectPoseUpdaterAgent agent to the pipeline
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="views">The list to add views to.</param>
        /// <param name="kinectMessageProducer">The kinect  message producer</param>
        /// <param name="jointMessageProducer">The joint message producer</param>        
        /// <returns>The joint message interpolator agent which interpolates joint messages at kinect message timestamps</returns>
        public static KinectPoseUpdaterAgent AddKinectPoseUpdater(Manifest manifest, List<ViewDefinition> views, IProducer<DepthCameraAgentMessage<KinectDepthCameraParameters>> kinectMessageProducer, IProducer<JointMessage> jointMessageProducer)
        {
            KinectPoseUpdaterAgent kinectPose = new KinectPoseUpdaterAgent("KinectPoseUpdated", kinectMessageProducer.Name, jointMessageProducer.Name, 0, 1);
            manifest.Agents.Add(kinectPose);
            return kinectPose;
        }

        /// <summary>
        /// Adds a PoseSLAMOptimizationAgent agent to the manifest
        /// </summary>
        /// <param name="filename">filename of the graph to load</param>
        /// <returns>a manifest</returns>
        public static Manifest AddPoseSLAMGraphOptimizationAgent(string filename)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            PoseSLAMGraphOptimizationTestAgent poseSLAM = new PoseSLAMGraphOptimizationTestAgent("PoseSLAMGraphOptimizationAgent", filename);
            GraphSLAMModelAgent graphSLAMModelAgent = new GraphSLAMModelAgent("GraphSlamModelAgent", poseSLAM, 100);

            ViewDefinition graphSlamViewer = new ViewDefinition(
                name: "GraphSLAMViewer",
                viewType: typeof(GraphSLAMViewer),
                modelAgent: graphSLAMModelAgent);

            // Add agents
            manifest.Agents.Add(poseSLAM);
            manifest.Agents.Add(graphSLAMModelAgent);

            // add the UI
            views.Add(graphSlamViewer);
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);

            // Activate agents
            manifest.ActiveAgents.Add(userInterface.Name);
            manifest.ActiveAgents.Add(poseSLAM.Name);

            return manifest;
        }

        /// <summary>
        /// Adds the global map (slam) UI models to the manifest and the global map UI views to the view list.
        /// </summary>
        /// <param name="manifest">The manifest to add models to.</param>
        /// <param name="views">The list to add views to.</param>
        /// <param name="slamAgent">The SLAM agent</param>
        /// <param name="hdp">Horizontal depth profile producer</param>
        /// <param name="maximumDepth">The cutoff depth for rendering the hdp</param>
        /// <param name="mapFile">Global map file</param>
        /// <param name="clickToLocalizeReceiver">Optional localization agent</param>
        /// <param name="name">Optional name, useful when more than one model of this type is added to the manifest</param>
        /// <typeparam name="THorizontalDepthProfile">The type of horizontal depth profile (laser or kinect)</typeparam>
        public static void AddGlobalMapModelView<THorizontalDepthProfile>(
            Manifest manifest,
            List<ViewDefinition> views,
            IProducer<SLAM2DAgentMessage> slamAgent,
            IProducer<SimpleAgentMessage<THorizontalDepthProfile>> hdp,
            double maximumDepth,
            string mapFile = null,
            IControlReceiver<SimpleAgentMessage<Pose2D>> clickToLocalizeReceiver = null,
            string name = "GlobalMapModelAgent")
            where THorizontalDepthProfile : IHorizontalDepthProfile
        {
            GlobalMapModelAgent<THorizontalDepthProfile> globalMapModel = new GlobalMapModelAgent<THorizontalDepthProfile>(
                name: name,
                mapProducer: slamAgent,
                hdpProducer: hdp,
                maximumDepth: maximumDepth,
                maxUpdateIntervalMilliseconds: DefaultVisualizationUpdateInterval * 2,
                mapFile: mapFile,
                clickToLocalizeReceiver: clickToLocalizeReceiver);
            manifest.Agents.Add(globalMapModel);

            ViewDefinition globalMapView = new ViewDefinition(
                name: name,
                viewType: typeof(GlobalMapView),
                modelAgent: globalMapModel);
            views.Add(globalMapView);
        }

        /// <summary>
        /// Adds the global map (slam) UI models to the manifest and the global map UI views to the view list, excluding the HDP overlay.
        /// </summary>
        /// <param name="manifest">The manifest to add models to.</param>
        /// <param name="views">The list to add views to.</param>
        /// <param name="slamAgent">The SLAM agent</param>
        /// <param name="mapFile">Global map file</param>
        public static void AddSimpleGlobalMapModelView(
            Manifest manifest,
            List<ViewDefinition> views,
            IProducer<SLAM2DAgentMessage> slamAgent,
            string mapFile = null)
        {
            SimpleGlobalMapModelAgent globalMapModel = new SimpleGlobalMapModelAgent(
                name: "GlobalMapModelAgent",
                mapProducer: slamAgent,
                maxUpdateIntervalMilliseconds: DefaultVisualizationUpdateInterval,
                mapFile: mapFile);
            manifest.Agents.Add(globalMapModel);

            ViewDefinition globalMapView = new ViewDefinition(
                name: "GlobalMap",
                viewType: typeof(GlobalMapView),
                modelAgent: globalMapModel);
            views.Add(globalMapView);
        }

        /// <summary>
        /// Adds the slam agent and corresponding UI .
        /// </summary>
        /// <param name="manifest">The manifest to add agents and models to.</param>
        /// <param name="views">The list to add views to.</param>
        /// <param name="hdp">The local map agent</param>
        /// <param name="driveFeedback">The agent generating drive feedback (odometry)</param>
        /// <param name="mapFile">The map to load.</param>
        /// <param name="pose">Start pose. Optional.</param>
        /// <param name="robotConfigFolder">Robot calibration folder.</param>
        /// <param name="maximumDepth">The depth cutoff for rendering overlaid HDPs</param>
        /// <param name="mapHeight">Map height. Optional, default is 800</param>
        /// <param name="mapWidth">Map width. Optional, default is 800</param>
        /// <param name="particleCount">Slam particle count</param>
        /// <param name="name">Optional name to give the agent.</param>
        /// <typeparam name="THorizontalDepthProfile">The type of horizontal depth profile (laser or kinect)</typeparam>
        /// <typeparam name="TDriveFeedbackState">The drive feedback state type used for motion update</typeparam>
        /// <returns>The slam agent.</returns>
        public static SLAM2DAgent<THorizontalDepthProfile, TDriveFeedbackState> AddSLAM2D<THorizontalDepthProfile, TDriveFeedbackState>(
            Manifest manifest,
            List<ViewDefinition> views,
            IProducer<SimpleAgentMessage<THorizontalDepthProfile>> hdp,
            IProducer<SimpleAgentMessage<TDriveFeedbackState>> driveFeedback,
            string mapFile,
            Pose2D pose,
            string robotConfigFolder,
            double maximumDepth,
            int mapHeight = 3000,
            int mapWidth = 3000,
            int particleCount = 100,
            string name = "SLAM2DAgent")
            where THorizontalDepthProfile : IHorizontalDepthProfile
            where TDriveFeedbackState : IDriveState
        {
            SLAM2DAgent<THorizontalDepthProfile, TDriveFeedbackState> slam = new SLAM2DAgent<THorizontalDepthProfile, TDriveFeedbackState>(
                name: name,
                hdpProducer: hdp,
                driveFeedbackProducer: driveFeedback,
                robotConfigFolder: robotConfigFolder,
                distanceResolution: GlobalMap.DefaultDistanceResolution,
                particleCount: particleCount,
                mapHeight: mapHeight,
                mapWidth: mapWidth,
                mapFile: mapFile,
                robotStartPose: pose);
            manifest.Agents.Add(slam);

            // add the model and view
            NavigationManifest.AddGlobalMapModelView<THorizontalDepthProfile>(manifest, views, slam, hdp, maximumDepth, mapFile, null, name + "Model");

            return slam;
        }

        /// <summary>
        /// Adds the static map localization agent and corresponding UI.
        /// </summary>
        /// <param name="manifest">The manifest to add agents and models to.</param>
        /// <param name="views">The list to add views to.</param>
        /// <param name="localMap">The local map agent.</param>
        /// <param name="driveFeedback">The agent generating drive feedback (odometry).</param>
        /// <param name="mapFile">The map to load.</param>
        /// <param name="startPose">The start pose of the robot.</param>
        /// <param name="startPoseVariance">The ambiguity in the start pose.</param>
        /// <param name="robotConfigFolder">Robot calibration folder</param>
        /// <param name="particleCount">Count of particles used in localization</param>
        /// <typeparam name="TDriveFeedbackState">The drive feedback state type used for motion update</typeparam>
        /// <returns>The slam agent.</returns>
        public static StaticMapLocalizationAgent<TDriveFeedbackState> AddStaticMapLocalization<TDriveFeedbackState>(
            Manifest manifest,
            List<ViewDefinition> views,
            IProducer<SimpleAgentMessage<KinectHorizontalDepthProfile>> localMap,
            IProducer<SimpleAgentMessage<TDriveFeedbackState>> driveFeedback,
            string mapFile,
            Pose2D startPose,
            Pose2D startPoseVariance,
            string robotConfigFolder,
            int particleCount = 200)
            where TDriveFeedbackState : IDriveState
        {
            var staticMapLoc = new StaticMapLocalizationAgent<TDriveFeedbackState>(
                name: "StaticMapLocalization",
                localMapProducer: localMap,
                driveFeedbackProducer: driveFeedback,
                robotConfigFolder: robotConfigFolder,
                distanceResolution: GlobalMap.DefaultDistanceResolution,
                particleCount: particleCount,
                mapFile: mapFile,
                robotStartPose: startPose,
                robotStartPoseVariance: startPoseVariance);
            manifest.Agents.Add(staticMapLoc);

            // add the model and view
            NavigationManifest.AddGlobalMapModelView(manifest, views, staticMapLoc, localMap, DepthImageConstants.MaximumRange, mapFile, staticMapLoc);

            return staticMapLoc;
        }

        /// <summary>
        /// Adds a remoting proxy to broadcast SLAM2D messages over the network.
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="robotPose">The agent to network-enable.</param>
        /// <param name="targetNameOrAddress">The name or IP address of the machine to broadcast to (e.g. a desktop machine)</param>
        /// <returns>A remoting proxy</returns>
        public static ProxyDefinition AddRobotMapPoseNetworkEnabledProxy(Manifest manifest, IProducer<SimpleAgentMessage<Pose2D>> robotPose, string targetNameOrAddress)
        {
            ProxyDefinition robotPoseProxy = ProxyDefinition.CreateNetworkEnabledProxy(
                name: robotPose.Name + "Proxy",
                producer: robotPose.Name,
                address: targetNameOrAddress,
                publishContracts: new[] { typeof(SimpleAgentMessage<Pose2D>) },
                publishNetworkPortNumber: (int)RemotingPorts.RobotMapPosePublish,
                controlContracts: null,
                controlNetworkPortNumber: 0);
            manifest.NetworkEnabledAgentProxies.Add(robotPoseProxy);

            return robotPoseProxy;
        }

        /// <summary>
        /// Adds a desktop proxy for a remote pose agent running on the robot.
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="views">The set of views to add to</param>
        /// <param name="robotNameOrAddress">The name or IP address of the robot</param>
        /// <returns>The robot pose proxy</returns>
        public static IProducer<SimpleAgentMessage<Pose2D>> AddRobotMapPoseRemoteProxy(Manifest manifest, List<ViewDefinition> views, string robotNameOrAddress)
        {
            // now create a proxy with the same name and add it to the manifest
            ProxyDefinition remoteProxy = ProxyDefinition.CreateRemoteProxy(
                name: "RobotPoseAgent",
                address: robotNameOrAddress,
                publishContracts: new[] { typeof(SimpleAgentMessage<Pose2D>) },
                publishNetworkPortNumber: (int)RemotingPorts.RobotMapPosePublish,
                controlContracts: null,
                controlNetworkPortNumber: 0);
            manifest.RemoteAgentProxies.Add(remoteProxy);

            return remoteProxy.AsProducer<SimpleAgentMessage<Pose2D>>();
        }

        /// <summary>
        /// Adds a remoting proxy to broadcast robot pose estimation agent messages over the network.
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="robotPoseEstimation">The agent to network-enable.</param>
        /// <param name="desktopNameOrAddress">The name or IP address of the machine to broadcast to (e.g. a desktop machine)</param>
        /// <returns>A remoting proxy</returns>
        public static ProxyDefinition AddRobotPoseEstimationNetworkEnabledProxy(Manifest manifest, IProducer<RobotPoseEstimationAgentMessage> robotPoseEstimation, string desktopNameOrAddress)
        {
            ProxyDefinition robotPoseEstimationProxy = ProxyDefinition.CreateNetworkEnabledProxy(
                name: robotPoseEstimation.Name + "Proxy",
                producer: robotPoseEstimation.Name,
                address: desktopNameOrAddress,
                publishContracts: new[] { typeof(RobotPoseEstimationAgentMessage) },
                publishNetworkPortNumber: (int)RemotingPorts.RobotPoseEstimationPublish,
                controlContracts: null,
                controlNetworkPortNumber: 0);
            manifest.NetworkEnabledAgentProxies.Add(robotPoseEstimationProxy);

            return robotPoseEstimationProxy;
        }

        /// <summary>
        /// Adds a desktop proxy for a robot pose estimation agent running on the robot.
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="robotNameOrAddress">The name or IP address of the robot</param>
        /// <returns>The proxy</returns>
        public static IProducer<RobotPoseEstimationAgentMessage> AddRobotPoseEstimationRemoteProxy(Manifest manifest, string robotNameOrAddress)
        {
            // now create a proxy with the same name and add it to the manifest
            ProxyDefinition robotPoseEstimationProxy = ProxyDefinition.CreateRemoteProxy(
                name: "RobotPoseEstimationAgent",
                address: robotNameOrAddress,
                publishContracts: new[] { typeof(RobotPoseEstimationAgentMessage) },
                publishNetworkPortNumber: (int)RemotingPorts.RobotPoseEstimationPublish,
                controlContracts: null,
                controlNetworkPortNumber: 0);
            manifest.RemoteAgentProxies.Add(robotPoseEstimationProxy);

            return robotPoseEstimationProxy.AsProducer<RobotPoseEstimationAgentMessage>();
        }

        /// <summary>
        /// Add ground truth from landmark agent to the manifest.
        /// </summary>
        /// <param name="manifest">Manifest to which the ground truth from landmark agent has to be added.</param>
        /// <param name="gamePadAgent">Game pad agent.</param>
        /// <param name="slam2DAgent">Slam 2D Agent.</param>
        /// <param name="recordingsDirectory">The recording directory containing the groundTruthLandmarkRelative.xml file</param>
        /// <returns>Ground Truth from marker agent.</returns>
        public static NavWaypointsFromSlamAgent AddNavWaypointsFromSlamAgent(
            Manifest manifest,
            IProducer<GamePadAgentMessage> gamePadAgent,
            IProducer<SLAM2DAgentMessage> slam2DAgent,
            string recordingsDirectory)
        {
            NavWaypointsFromSlamAgent waypointsFromLandmarkAgent = new NavWaypointsFromSlamAgent(
                "NavWaypointsFromSlam",
                gamePadAgent,
                slam2DAgent,
                recordingsDirectory);
            manifest.Agents.Add(waypointsFromLandmarkAgent);
            return waypointsFromLandmarkAgent;
        }

        /// <summary>
        /// Adds a graph optimizer
        /// </summary>
        /// <param name="manifest">manifest to add it too</param>
        /// <param name="views">view list</param>
        /// <param name="recordingsDirectory">directory of the recording</param>
        /// <param name="mapWidth">width of the map</param>
        /// <param name="mapHeight">height of the map</param>
        /// <param name="startPose">start pose</param>
        /// <param name="hdpWithPose">producer of HDP with pose messages</param>
        /// <param name="gamepad">producer of gamepad messages</param>
        /// <param name="completion">producer of completion messages</param>
        /// <param name="useGroundTruth">use ground truth file for constraints</param>
        /// <param name="numberOfYButtons">The number of Y buttons expected</param>
        /// <returns>A graph optimization agent</returns>
        public static GraphOptimizationPose2DAgent<KinectHorizontalDepthProfile> Add2DGraphOptimizer(
            Manifest manifest,
            List<ViewDefinition> views,
            string recordingsDirectory,
            int mapWidth,
            int mapHeight,
            Pose2D startPose,
            IProducer<HDPwithPoseMessage<KinectHorizontalDepthProfile>> hdpWithPose,
            IProducer<GamePadAgentMessage> gamepad,
            IProducer<CompletionMessage> completion,
            bool useGroundTruth = false,
            int numberOfYButtons = 0)
        {
            LoopClosingAgent loopClosingAgent = new LoopClosingAgent(
                name: "LoopClosingAgent", 
                slamProducer: hdpWithPose, 
                gamePadProducer: gamepad,
                completionMessageProducer: completion,
                groundTruthFile: Path.Combine(recordingsDirectory, "GroundTruth.txt"),
                groundTruthAbsoluteFile: useGroundTruth ? Path.Combine(recordingsDirectory, "GroundTruthAbsolute.txt") : null,
                expectedYbuttons: numberOfYButtons);
            manifest.Agents.Add(loopClosingAgent);

            GraphOptimizationPose2DAgent<KinectHorizontalDepthProfile> graphOptimizationAgent =
                new GraphOptimizationPose2DAgent<KinectHorizontalDepthProfile>(
                    "GraphAgent",
                    hdpWithPose,
                    loopClosingAgent,
                    mapWidth,
                    mapHeight,
                    startPose,
                    "map.xml");
            manifest.Agents.Add(graphOptimizationAgent);

            // add the UI
            GraphSLAMModelAgent graphModelAgent = new GraphSLAMModelAgent("GraphSLAMModel", graphOptimizationAgent, 100);
            
            ViewDefinition graphSlamViewer = new ViewDefinition("GraphSLAMViewer", typeof(GraphSLAMViewer), graphModelAgent);
            
            manifest.Agents.Add(graphModelAgent);
            views.Add(graphSlamViewer);

            return graphOptimizationAgent;
        }

        /// <summary>
        /// Creates a manifest that can be used to record EV4 sensor data
        /// </summary>
        /// <param name="robotConfigFolder">Robot Calibration Data Folder</param>
        /// <param name="environmentConfigFolder">Environment config folder</param>
        /// <param name="waypointFile">Path to the file containing waypoints for this SLAM recording</param>
        /// <param name="outputDir">Path to the directory to which ground truth files will be written.</param>
        /// <returns>A manifest for recording EV4 sensor data</returns>
        /// <remarks>If you don't want to create ground truth files, specify null or empty string for the waypoint file and/or output directory</remarks>
        public static Manifest CreateEV4RecorderManifest(string robotConfigFolder, string environmentConfigFolder, string waypointFile, string outputDir)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the agents
            HeartbeatAgent heartbeat = EV4Manifest.AddEV4HeartbeatAgent(manifest, views, robotConfigFolder);
            KinectAgent kinect = VisionManifest.AddEV4Kinect(manifest, views);

            GamePadAgent gamepad = InteractionManifest.AddGamePadAgent(manifest, views);

            DifferentialDriveTrackingAgent tracking = NavigationManifest.AddSlamMapperDriveAndDependencies(manifest, views, gamepad, heartbeat, heartbeat, kinect, robotConfigFolder);

            GroundTruthLoggerAgent groundTruthLogger = null;
            FileRecorderProxy rgbCameraRecorder = null;

            if (!string.IsNullOrEmpty(outputDir) && !string.IsNullOrEmpty(waypointFile))
            {
                IProducer<RGBCameraAgentMessage> rgbCameraAgent = VisionManifest.AddRGBCameraAgent(manifest, views, 3, 1);
                IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimation = VisionManifest.AddCameraPoseEstimationAgent(rgbCameraAgent, manifest, views, robotConfigFolder, environmentConfigFolder);
                IProducer<RobotPoseEstimationAgentMessage> robotPoseEstimation = AddRobotPoseEstimationAgent(rgbCameraAgent, cameraPoseEstimation, manifest, views, robotConfigFolder);
                groundTruthLogger = AddGroundTruthLogger(robotConfigFolder, outputDir, waypointFile, manifest, views, gamepad, cameraPoseEstimation, robotPoseEstimation);
                rgbCameraRecorder = VisionManifest.AddRGBCameraRecorder(manifest, rgbCameraAgent);
            }

            // add the recorders
            FileRecorderProxy heartbeatRecorder = EV4Manifest.AddEV4HeartbeatRecorder(manifest, heartbeat);
            VisionManifest.AddKinectRecorder(manifest, kinect);
            FileRecorderProxy gamepadRecorder = InteractionManifest.AddGamePadRecorder(manifest, gamepad);

            // add the UI
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(tracking.Name);
            manifest.ActiveAgents.Add(heartbeatRecorder.Name);
            manifest.ActiveAgents.Add(gamepadRecorder.Name);
            manifest.ActiveAgents.Add(userInterface.Name);

            if (groundTruthLogger != null)
            {
                manifest.ActiveAgents.Add(rgbCameraRecorder.Name);
                manifest.ActiveAgents.Add(groundTruthLogger.Name);
            }

            return manifest;
        }

        /// <summary>
        /// Creates a manifest that can be used to broadcast EV4 sensor data over the network. The robot can be controlled with a remote gamepad.
        /// </summary>
        /// <param name="targetNameOrAddress">The IP address of the machine to broadcast to (e.g. a desktop machine)</param>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <param name="environmentConfigFolder">Environment config folder</param>
        /// <param name="waypointFile">Path to the file containing waypoints for this SLAM recording</param>
        /// <param name="outputDir">Path to the directory to which ground truth files will be written.</param>
        /// <returns>A manifest for broadcasting EV4 sensor data</returns>
        public static Manifest CreateEV4RemoteProducerManifest(string targetNameOrAddress, string robotConfigFolder, string environmentConfigFolder, string waypointFile, string outputDir)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the agents
            HeartbeatAgent heartbeat = EV4Manifest.AddEV4HeartbeatAgent(manifest, views, robotConfigFolder);
            DifferentialDriveAgent driveFeedback = NavigationManifest.AddDifferentialDrive(manifest, views, heartbeat, robotConfigFolder);
            VirtualDriveFeedbackAgent virtualDrive = NavigationManifest.AddVirtualDriveFeedback(manifest, views, driveFeedback, heartbeat, robotConfigFolder);

            KinectAgent kinect = VisionManifest.AddEV4Kinect(manifest, views);
            KinectAgentMessageProcessor processor = VisionManifest.AddKinectProcessor(manifest, views, kinect, robotConfigFolder);
            JointMessageInterpolatorAgent jointMessageInterpolator = NavigationManifest.AddJointMessageInterpolator(manifest, views, heartbeat, processor);
            GyroscopeMessageInterpolatorAgent gyroscopeMessageInterpolator = NavigationManifest.AddGyroscopeMessageInterpolator(manifest, views, heartbeat, processor);
            DifferentialDriveMessageInterpolatorAgent differentialDriveMessageInterpolator = NavigationManifest.AddDifferentialDriveMessageInterpolator(manifest, views, heartbeat, processor);
            KinectPoseUpdaterAgent kinectPose = NavigationManifest.AddKinectPoseUpdater(manifest, views, processor, jointMessageInterpolator);
            KinectHorizontalDepthProfileAgent hdp = NavigationManifest.AddKinectHorizontalDepthProfile(manifest, views, kinectPose, robotConfigFolder);
            IProducer<GamePadAgentMessage> gamepad = InteractionManifest.AddRemoteGamePadAgentProxy(manifest, views, targetNameOrAddress);

            SlamMapperDriveAgent slamdriveAgent = NavigationManifest.AddSlamMapperDrive(manifest, views, robotConfigFolder, gamepad, heartbeat, kinect, driveFeedback);
            DifferentialDriveTrackingAgent tracking = NavigationManifest.AddDifferentialDriveTracking(manifest, views, slamdriveAgent, heartbeat);
            IProducer<RGBCameraAgentMessage> rgbCameraAgent = VisionManifest.AddRGBCameraAgent(manifest, views, 3, 1);

            GroundTruthLoggerAgent groundTruthLogger = null;

            if (!string.IsNullOrEmpty(outputDir) && !string.IsNullOrEmpty(waypointFile))
            {
                IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimation = VisionManifest.AddCameraPoseEstimationAgent(rgbCameraAgent, manifest, views, robotConfigFolder, environmentConfigFolder);
                IProducer<RobotPoseEstimationAgentMessage> robotPoseEstimation = AddRobotPoseEstimationAgent(rgbCameraAgent, cameraPoseEstimation, manifest, views, robotConfigFolder);
                groundTruthLogger = AddGroundTruthLogger(robotConfigFolder, outputDir, waypointFile, manifest, views, gamepad, cameraPoseEstimation, robotPoseEstimation);
            }

            // add the recorders
            FileRecorderProxy heartbeatRecorder = EV4Manifest.AddEV4HeartbeatRecorder(manifest, heartbeat);
            VisionManifest.AddKinectRecorder(manifest, kinect);
            FileRecorderProxy gamepadRecorder = InteractionManifest.AddGamePadRecorder(manifest, gamepad);
            FileRecorderProxy rgbCameraRecorder = VisionManifest.AddRGBCameraRecorder(manifest, rgbCameraAgent);

            // add the remote proxies
            ProxyDefinition driveProxy = NavigationManifest.AddDriveFeedbackNetworkEnabledProxy(manifest, virtualDrive, targetNameOrAddress);
            ProxyDefinition hdpProxy = NavigationManifest.AddKinectHorizontalDepthProfileNetworkEnabledProxy(manifest, hdp, targetNameOrAddress);

            // add the UI
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(tracking.Name);
            manifest.ActiveAgents.Add(heartbeatRecorder.Name);
            manifest.ActiveAgents.Add(gamepadRecorder.Name);
            manifest.ActiveAgents.Add(rgbCameraRecorder.Name);
            manifest.ActiveAgents.Add(virtualDrive.Name);
            manifest.ActiveAgents.Add(driveProxy.Name);
            manifest.ActiveAgents.Add(hdp.Name);
            manifest.ActiveAgents.Add(hdpProxy.Name);
            manifest.ActiveAgents.Add(userInterface.Name);

            if (groundTruthLogger != null)
            {
                manifest.ActiveAgents.Add(groundTruthLogger.Name);
            }

            return manifest;
        }

        /// <summary>
        /// Creates a kinect-only manifest that enables all the kinect visualizations 
        /// </summary>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <returns>A manifest that enables kinect and its visualizations</returns>
        public static Manifest CreateKinectTestManifest(string robotConfigFolder)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the agents 
            KinectAgent kinect = VisionManifest.AddEV4Kinect(manifest, views);
            KinectAgentMessageProcessor processor = VisionManifest.AddKinectProcessor(manifest, views, kinect, robotConfigFolder);
            KinectHorizontalDepthProfileAgent hdp = NavigationManifest.AddKinectHorizontalDepthProfile(manifest, views, processor, robotConfigFolder);

            // add the UI
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(hdp.Name);
            manifest.ActiveAgents.Add(userInterface.Name);

            return manifest;
        }

        /// <summary>
        /// Create a manifest to estimate the robot pose from RGB images
        /// </summary>
        /// <param name="robotConfigFolder">Calibration Folder</param>
        /// <param name="environmentConfigFolder">Environment calibration folder</param>
        /// <returns>A manifest that extracts ground truth estimate from RGB images</returns>
        public static Manifest CreateRobotPoseEstimationManifest(string robotConfigFolder, string environmentConfigFolder)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // ground truth (intentionally not added to active agents)
            IProducer<RGBCameraAgentMessage> rgbCamera = VisionManifest.AddRGBCameraAgent(manifest, views, 30, 1);
            IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimation;
            IProducer<RobotPoseEstimationAgentMessage> robotPoseEstimation;
            cameraPoseEstimation = VisionManifest.AddCameraPoseEstimationAgent(rgbCamera, manifest, views, robotConfigFolder, environmentConfigFolder);
            robotPoseEstimation = AddRobotPoseEstimationAgent(rgbCamera, cameraPoseEstimation, manifest, views, robotConfigFolder);

            // add the UI
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);
            manifest.ActiveAgents.Add(userInterface.Name);

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(userInterface.Name);
            manifest.ActiveAgents.Add(rgbCamera.Name);
            manifest.ActiveAgents.Add(cameraPoseEstimation.Name);
            manifest.ActiveAgents.Add(robotPoseEstimation.Name);

            return manifest;
        }

        /// <summary>
        /// Add robot pose estimation agent to the manifest.
        /// </summary>
        /// <param name="rgbCamera">RGB camera agent</param>
        /// <param name="cameraPoseEstimation">Camera pose estimation agent</param>
        /// <param name="manifest">Manifest to which the robot pose estimation agent should be added.</param>
        /// <param name="views">View to which the robot pose estimation is to be added.</param>
        /// <param name="robotConfigFolder">Robot Calibration folder.</param>
        /// <param name="addUI">Whether to add the UI corresponding to the agent.</param>
        /// <returns>RobotPose Estimation Agent</returns>
        public static IProducer<RobotPoseEstimationAgentMessage> AddRobotPoseEstimationAgent(
            IProducer<RGBCameraAgentMessage> rgbCamera,
            IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimation,
            Manifest manifest,
            List<ViewDefinition> views,
            string robotConfigFolder,
            bool addUI = true)
        {
            RobotPoseEstimationAgent robotPoseEstimation = new RobotPoseEstimationAgent(
                name: "RobotPoseEstimationAgent",
            producer: cameraPoseEstimation,
            calibrationDataExportFolder: robotConfigFolder);
            manifest.Agents.Add(robotPoseEstimation);

            if (addUI)
            {
                RobotPoseEstimationModelAgent robotPoseEstimationModel = new RobotPoseEstimationModelAgent(
                  name: "RobotPoseEstimationModelAgentRGB",
                  rgbImageProducer: rgbCamera,
                  robotPoseProducer: robotPoseEstimation,
                  maxUpdateIntervalMilliseconds: 16,
                  imageFormat: SimplePixelFormat.Gray8);
                manifest.Agents.Add(robotPoseEstimationModel);

                ViewDefinition def = new ViewDefinition("Robot Pose Estimation View", typeof(RobotPoseEstimationView), robotPoseEstimationModel);
                views.Add(def);
            }

            return robotPoseEstimation;
        }

        /// <summary>
        /// Adds a ground truth logger agent to the specified manifest
        /// </summary>
        /// <param name="robotCalibrationFolder">Path to the calibration folder</param>
        /// <param name="outputDir">Directory to write ground truth files</param>
        /// <param name="waypointFile">Path to the waypoint file</param>
        /// <param name="manifest">The manifest that will be added to</param>
        /// <param name="views">The view list that will be added to</param>
        /// <param name="gamepad">The game pad agent</param>
        /// <param name="cameraPoseEstimation">The camera pose estimation agent</param>
        /// <param name="robotPoseEstimation">The robot pose estimation agent</param>
        /// <returns>A GroundTruthLoggerAgent</returns>
        public static GroundTruthLoggerAgent AddGroundTruthLogger(string robotCalibrationFolder, string outputDir, string waypointFile, Manifest manifest, List<ViewDefinition> views, IProducer<GamePadAgentMessage> gamepad, IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimation, IProducer<RobotPoseEstimationAgentMessage> robotPoseEstimation)
        {
            GroundTruthLoggerAgent groundTruthLogger = new GroundTruthLoggerAgent(
                name: "GroundTruthLogger",
                outputDir: outputDir,
                gamePadProducer: gamepad.Name,
                cameraPoseProducer: cameraPoseEstimation.Name,
                robotPoseProducer: robotPoseEstimation.Name,
                waypointFile: waypointFile);
            manifest.Agents.Add(groundTruthLogger);

            return groundTruthLogger;
        }

        /// <summary>
        /// Creates a manifest for running SLAM on an existing recording
        /// </summary>
        /// <param name="recordingsDirectory">The directory that contains all the recordings</param>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <param name="mapHeight">The height of the map</param>
        /// <param name="mapWidth">THE width of the map</param>
        /// <param name="startPose">The start pose on the map, relative to the bottom right corner, in map units.</param>
        /// <param name="particleCount">The number of particles to use</param>
        /// <param name="mapFile">The map file to record to</param>
        /// <param name="targetPath">If set, SLAM and HDP are recorded to this location</param>
        /// <returns>A manifest that plays back recordings and runs SLAM</returns>
        public static Manifest CreateSLAMPlaybackManifest(
            string recordingsDirectory,
            string robotConfigFolder,
            int mapHeight,
            int mapWidth,
            Pose2D startPose,
            int particleCount,
            string mapFile = null,
            string targetPath = null)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the file players
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views);
            FilePlayerProxy gamepad = InteractionManifest.AddGamePadPlayback(manifest, views, timer, recordingsDirectory);

            DepthImageDecompressionAgent<KinectDepthCameraParameters> kinectDepth = VisionManifest.AddDepthPlayback<KinectDepthCameraParameters>(
                manifest, 
                timer, 
                recordingsDirectory, 
                "KinectCompressedDepthAgent.xml");

            FilePlayerProxy heartbeat = EV4Manifest.AddEV4HeartbeatPlayback(manifest, views, timer, recordingsDirectory);

            // add the agents 
            KinectAgentMessageProcessor processor = VisionManifest.AddKinectProcessor(manifest, views, kinectDepth, robotConfigFolder);
            JointMessageInterpolatorAgent jointMessageInterpolator = NavigationManifest.AddJointMessageInterpolator(manifest, views, heartbeat.AsProducer<JointMessage>(), processor);
            GyroscopeMessageInterpolatorAgent gyroscopeMessageInterpolator = NavigationManifest.AddGyroscopeMessageInterpolator(manifest, views, heartbeat.AsProducer<GyroscopeMessage>(), processor);
            DifferentialDriveMessageInterpolatorAgent differentialDriveMessageInterpolator = NavigationManifest.AddDifferentialDriveMessageInterpolator(manifest, views, heartbeat.AsProducer<DifferentialDriveMessage>(), processor);
            KinectPoseUpdaterAgent kinectPose = NavigationManifest.AddKinectPoseUpdater(manifest, views, processor, jointMessageInterpolator);

            DifferentialDriveAgent driveFeedback = NavigationManifest.AddDifferentialDrive(manifest, views, differentialDriveMessageInterpolator, robotConfigFolder);
            VirtualDriveFeedbackAgent virtualDrive = NavigationManifest.AddVirtualDriveFeedback(manifest, views, driveFeedback, gyroscopeMessageInterpolator, robotConfigFolder);
            KinectHorizontalDepthProfileAgent hdp = NavigationManifest.AddKinectHorizontalDepthProfile(manifest, views, kinectPose, robotConfigFolder);

            SLAM2DAgent<KinectHorizontalDepthProfile, DriveFeedbackState> slam = NavigationManifest.AddSLAM2D(manifest, views, hdp, virtualDrive, mapFile, startPose, robotConfigFolder, DepthImageConstants.MaximumRange, mapHeight, mapWidth, particleCount);

            if (targetPath != null)
            {
                FileRecorderProxy hdpRecorder = new FileRecorderProxy("HDPRecorder", targetPath, new[] { typeof(SimpleAgentMessage<KinectHorizontalDepthProfile>) }, hdp.Name);
                manifest.Agents.Add(hdpRecorder);
                manifest.ActiveAgents.Add(hdpRecorder.Name);
                FileRecorderProxy slamRecorder = new FileRecorderProxy("SLAMRecorder", targetPath, new[] { typeof(SLAM2DAgentMessage) }, slam.Name);
                manifest.Agents.Add(slamRecorder);
                manifest.ActiveAgents.Add(slamRecorder.Name);
                FileRecorderProxy hdpWithPoseRecorder = new FileRecorderProxy("HDPwithPoseRecorder", targetPath, new[] { typeof(HDPwithPoseMessage<KinectHorizontalDepthProfile>) }, slam.Name);
                manifest.Agents.Add(hdpWithPoseRecorder);
                manifest.ActiveAgents.Add(hdpWithPoseRecorder.Name);
            }

            // add the UI
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(slam.Name);
            manifest.ActiveAgents.Add(userInterface.Name);

            return manifest;
        }

        /// <summary>
        /// Creates a manifest for running SLAM on an existing recording
        /// </summary>
        /// <param name="recordingsDirectory">The directory that contains all the recordings</param>
        /// <param name="robotConfigFolder">Directory of the robot configuration file</param>
        /// <param name="mapHeight">The height of the map</param>
        /// <param name="mapWidth">THE width of the map</param>
        /// <param name="startPose">The start pose on the map, relative to the bottom right corner, in map units.</param>
        /// <param name="useGroundTruth">Use the ground truth for constraints</param>
        /// <param name="runMetrics">Run the metrics</param>
        /// <param name="expectedYButtons">Expected number of Y buttons</param>
        /// <returns>A manifest that plays back recordings and runs the Graph Optimization</returns>
        public static Manifest CreateGraphOptimizationPlaybackManifest(
            string recordingsDirectory,
            string robotConfigFolder,
            int mapHeight,
            int mapWidth,
            Pose2D startPose,
            bool useGroundTruth = false,
            bool runMetrics = false,
            int expectedYButtons = 0)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the file players
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views, 1, 100);
            FilePlayerProxy gamepad = InteractionManifest.AddGamePadPlayback(manifest, views, timer, recordingsDirectory);
           
            FilePlayerProxy hdpWithPose = new FilePlayerProxy(  
                "HdpWithPose", 
                Path.Combine(recordingsDirectory, "HDPWithPoseRecorder.xml"), 
                new[] { typeof(HDPwithPoseMessage<KinectHorizontalDepthProfile>) }, 
                timer);
            manifest.Agents.Add(hdpWithPose);

            GraphOptimizationPose2DAgent<KinectHorizontalDepthProfile> graphOptimizationAgent = Add2DGraphOptimizer(
                manifest: manifest, 
                views: views,
                recordingsDirectory: recordingsDirectory,
                mapWidth: mapWidth, 
                mapHeight: mapHeight, 
                startPose: startPose, 
                hdpWithPose: hdpWithPose.AsProducer<HDPwithPoseMessage<KinectHorizontalDepthProfile>>(), 
                gamepad: gamepad.AsProducer<GamePadAgentMessage>(), 
                completion: gamepad.AsProducer<CompletionMessage>(),
                useGroundTruth: useGroundTruth,
                numberOfYButtons: expectedYButtons);

            FileRecorderProxy trajectoryRecorder = new FileRecorderProxy("OptimizedTrajectory", recordingsDirectory, new[] { typeof(SLAM2DAgentMessage) }, graphOptimizationAgent.Name);
            manifest.Agents.Add(trajectoryRecorder);
            manifest.ActiveAgents.Add(trajectoryRecorder.Name);

            // Get the metrics
            if (runMetrics)
            {
                FilePlayerProxy slamPlayer = new FilePlayerProxy(
                    "SlamPlayer",
                    Path.Combine(recordingsDirectory, "SLAMRecorder.xml"),
                    new[] { typeof(SLAM2DAgentMessage) },
                    timer);
                manifest.Agents.Add(slamPlayer);

                GroundTruthFromStopAndGoFileAgent groundTruth = new GroundTruthFromStopAndGoFileAgent(
                    name: "GroundTruthAgent",
                    producer: gamepad.AsProducer<GamePadAgentMessage>(),
                    groundTruthFile: Path.Combine(recordingsDirectory, "GroundTruth.txt"));
                manifest.Agents.Add(groundTruth);

                SLAM2DToPose2DTransformerAgent slam2DToPose2D = NavigationManifest.AddSlam2DToPose2DTransformerAgent(manifest, views, graphOptimizationAgent);
                PositioningPrecisionCalculator calc = NavigationManifest.AddPositioningPrecisionCalculator(manifest, views, slam2DToPose2D, groundTruth, graphOptimizationAgent, 10000);

                MetricsController controller = new MetricsController(
                    name: "controller",
                    calculatorAgent: calc,
                    completionAgents: new IProducer<CompletionMessage>[0], // no completion agents.  Calculator will indicate completion via metrics message.
                    iterations: 1);
                manifest.Agents.Add(controller);
                manifest.ActiveAgents.Add(controller.Name);
            }

            // add the UI
            // add the model and view
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);

            // specify the leaf agents to activate
            if (runMetrics == false)
            {
                manifest.ActiveAgents.Add(graphOptimizationAgent.Name);
            }
            
            manifest.ActiveAgents.Add(userInterface.Name);

            return manifest;
        }

        /// <summary>
        /// Creates a manifest for running SLAM. optimizing the output, and getting the metrics on an existing recording
        /// </summary>
        /// <param name="recordingsDirectory">The directory that contains all the recordings</param>
        /// <param name="robotConfigFolder">Directory of the robot configuration file</param>
        /// <param name="mapHeight">The height of the map</param>
        /// <param name="mapWidth">THE width of the map</param>
        /// <param name="startPose">The start pose on the map, relative to the bottom right corner, in map units.</param>
        /// <param name="particleCount">Number of particles to use</param>
        /// <param name="runs">Number of runs to use</param>
        /// <returns>A manifest that plays SLAM and optimizes the output and gets metrics</returns>
        public static Manifest CreateSLAMWithGraphOptimizationMetricsManifest(
            string recordingsDirectory,
            string robotConfigFolder,
            int mapHeight,
            int mapWidth,
            Pose2D startPose,
            int particleCount = 50,
            int runs = 10)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the file players
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views, 1, 100); // run as fast as /o will let us
            FilePlayerProxy gamepad = InteractionManifest.AddGamePadPlayback(manifest, views, timer, recordingsDirectory);

            DepthImageDecompressionAgent<KinectDepthCameraParameters> kinectDepth = VisionManifest.AddDepthPlayback<KinectDepthCameraParameters>(
                manifest,
                timer,
                recordingsDirectory,
                "KinectCompressedDepthAgent.xml");

            FilePlayerProxy heartbeat = EV4Manifest.AddEV4HeartbeatPlayback(manifest, views, timer, recordingsDirectory);

            // add the SLAM agents
            KinectAgentMessageProcessor processor = VisionManifest.AddKinectProcessor(manifest, views, kinectDepth, robotConfigFolder);
            JointMessageInterpolatorAgent jointMessageInterpolator = NavigationManifest.AddJointMessageInterpolator(manifest, views, heartbeat.AsProducer<JointMessage>(), processor);
            GyroscopeMessageInterpolatorAgent gyroscopeMessageInterpolator = NavigationManifest.AddGyroscopeMessageInterpolator(manifest, views, heartbeat.AsProducer<GyroscopeMessage>(), processor);
            DifferentialDriveMessageInterpolatorAgent differentialDriveMessageInterpolator = NavigationManifest.AddDifferentialDriveMessageInterpolator(manifest, views, heartbeat.AsProducer<DifferentialDriveMessage>(), processor);
            KinectPoseUpdaterAgent kinectPose = NavigationManifest.AddKinectPoseUpdater(manifest, views, processor, jointMessageInterpolator);
            DifferentialDriveAgent driveFeedback = NavigationManifest.AddDifferentialDrive(manifest, views, differentialDriveMessageInterpolator, robotConfigFolder);
            VirtualDriveFeedbackAgent virtualDrive = NavigationManifest.AddVirtualDriveFeedback(manifest, views, driveFeedback, gyroscopeMessageInterpolator, robotConfigFolder);
            KinectHorizontalDepthProfileAgent hdp = NavigationManifest.AddKinectHorizontalDepthProfile(manifest, views, kinectPose, robotConfigFolder);
            SLAM2DAgent<KinectHorizontalDepthProfile, DriveFeedbackState> slam = NavigationManifest.AddSLAM2D(manifest, views, hdp, virtualDrive, null, startPose, robotConfigFolder, DepthImageConstants.MaximumRange, mapHeight, mapWidth, particleCount);
            GraphOptimizationPose2DAgent<KinectHorizontalDepthProfile> graphOptimizationAgent = NavigationManifest.Add2DGraphOptimizer(manifest, views, recordingsDirectory, mapWidth, mapHeight, startPose, slam, gamepad.AsProducer<GamePadAgentMessage>(), gamepad.AsProducer<CompletionMessage>(), false);

            // Get the metrics
            GroundTruthFromStopAndGoFileAgent groundTruth = new GroundTruthFromStopAndGoFileAgent(
                name: "GroundTruthAgent",
                producer: gamepad.AsProducer<GamePadAgentMessage>(),
                groundTruthFile: Path.Combine(recordingsDirectory, "GroundTruth.txt"));
            manifest.Agents.Add(groundTruth);

            SLAM2DToPose2DTransformerAgent slam2DToPose2D = NavigationManifest.AddSlam2DToPose2DTransformerAgent(manifest, views, graphOptimizationAgent);
            PositioningPrecisionCalculator calc = NavigationManifest.AddPositioningPrecisionCalculator(manifest, views, slam2DToPose2D, groundTruth, graphOptimizationAgent, 10000);

            MetricsController controller = new MetricsController(
                name: "controller",
                calculatorAgent: calc,
                completionAgents: new IProducer<CompletionMessage>[0], // no completion agents.  Calculator will indicate completion via metrics message.
                iterations: runs);
            manifest.Agents.Add(controller);
            manifest.ActiveAgents.Add(controller.Name);
        
            ////// add the UI
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(userInterface.Name);
            
            return manifest;
        }

        /// <summary>
        /// Creates a manifest that runs localization on the robot. The robot can be controlled with a remote gamepad.
        /// </summary>
        /// <param name="targetNameOrAddress">The IP address of the machine to broadcast to (e.g. a desktop machine)</param>
        /// <param name="startPose">The start pose on the map, relative to the bottom right corner, in map units.</param>
        /// <param name="startPoseVariance">The variance of the start pose.</param>
        /// <param name="robotConfigFolder">Robot calibration folder</param>
        /// <returns>A manifest that enables kinect and its visualizations</returns>
        /// <returns>A manifest for broadcasting EV4 sensor data</returns>
        public static Manifest CreateLocalizationManifest(string targetNameOrAddress, Pose2D startPose, Pose2D startPoseVariance, string robotConfigFolder)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the agents 
            HeartbeatAgent heartbeat = EV4Manifest.AddEV4HeartbeatAgent(manifest, views, robotConfigFolder);
            DifferentialDriveAgent driveFeedback = NavigationManifest.AddDifferentialDrive(manifest, views, heartbeat, robotConfigFolder);
            VirtualDriveFeedbackAgent virtualDrive = NavigationManifest.AddVirtualDriveFeedback(manifest, views, driveFeedback, heartbeat, robotConfigFolder);

            KinectAgent kinect = VisionManifest.AddEV4Kinect(manifest, views);
            KinectAgentMessageProcessor processor = VisionManifest.AddKinectProcessor(manifest, views, kinect, robotConfigFolder);
            KinectPoseUpdaterAgent kinectPose = NavigationManifest.AddKinectPoseUpdater(manifest, views, processor, heartbeat);
            KinectHorizontalDepthProfileAgent hdp = NavigationManifest.AddKinectHorizontalDepthProfile(manifest, views, kinectPose, robotConfigFolder);
            IProducer<GamePadAgentMessage> gamepad = InteractionManifest.AddRemoteGamePadAgentProxy(manifest, views, targetNameOrAddress);
            SlamMapperDriveAgent slamdriveAgent = NavigationManifest.AddSlamMapperDrive(manifest, views, robotConfigFolder, gamepad, heartbeat, kinect, virtualDrive);
            DifferentialDriveTrackingAgent tracking = NavigationManifest.AddDifferentialDriveTracking(manifest, views, slamdriveAgent, heartbeat);

            // add the loc agent
            StaticMapLocalizationAgent<DriveFeedbackState> localization = NavigationManifest.AddStaticMapLocalization<DriveFeedbackState>(
                 manifest,
                 views,
                 hdp,
                 virtualDrive,
                 @"c:\temp\map.xml",
                 startPose,
                 startPoseVariance,
                 robotConfigFolder);

            // add the UI
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(gamepad.Name);
            manifest.ActiveAgents.Add(tracking.Name);
            manifest.ActiveAgents.Add(virtualDrive.Name);

            manifest.ActiveAgents.Add(localization.Name);
            manifest.ActiveAgents.Add(userInterface.Name);

            return manifest;
        }

        /// <summary>
        /// Creates a manifest for running SLAM on a desktop connected remotely to a robot (the robot should run EV4RemoteProducer)
        /// </summary>
        /// <param name="robotNameOrAddress">The name or IP address of the robot to connect to.</param>
        /// <param name="startPose">The start pose on the map, relative to the bottom right corner, in map units.</param>
        /// <param name="startPoseVariance">The variance of the start pose.</param>
        /// <param name="robotConfigFolder">Robot calibration folder</param>
        /// <returns>A manifest that enables kinect and its visualizations</returns>
        public static Manifest CreateLocalizationRemoteConsumerManifest(string robotNameOrAddress, Pose2D startPose, Pose2D startPoseVariance, string robotConfigFolder)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the remote proxies
            IProducer<SimpleAgentMessage<DriveFeedbackState>> virtualDrive = NavigationManifest.AddDriveFeedbackRemoteProxy(manifest, views, robotNameOrAddress);
            IProducer<SimpleAgentMessage<KinectHorizontalDepthProfile>> hdp = NavigationManifest.AddKinectHorizontalDepthProfileRemoteProxy(manifest, views, robotNameOrAddress);
            NavigationManifest.AddHorizontalDepthProfileModelView(manifest, views, hdp, 0.02, 10);

            // add the agents 
            StaticMapLocalizationAgent<DriveFeedbackState> localization = NavigationManifest.AddStaticMapLocalization(
                 manifest,
                 views,
                 hdp,
                 virtualDrive,
                 @"c:\temp\map.xml",
                 startPose,
                 startPoseVariance,
                 robotConfigFolder,
                 250);

            // add the UI
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(localization.Name);
            manifest.ActiveAgents.Add(userInterface.Name);

            return manifest;
        }

        /// <summary>
        /// Creates a manifest for running SLAM on a desktop connected remotely to a robot (the robot should run EV4RemoteProducer)
        /// </summary>
        /// <param name="robotNameOrAddress">The name or IP address of the robot to connect to.</param>
        /// <param name="startPose">The start pose on the map, relative to the bottom right corner, in map units.</param>
        /// <param name="robotConfigFolder">Robot calibration folder</param>
        /// <returns>A manifest that enables kinect and its visualizations</returns>
        public static Manifest CreateSLAMRemoteConsumerManifest(string robotNameOrAddress, Pose2D startPose, string robotConfigFolder)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the remote proxies
            HeartbeatAgent heartbeat = EV4Manifest.AddEV4RemoteProxy(manifest, views, robotNameOrAddress, robotConfigFolder);
            IProducer<SimpleAgentMessage<KinectHorizontalDepthProfile>> hdp = NavigationManifest.AddKinectHorizontalDepthProfileRemoteProxy(manifest, views, robotNameOrAddress);

            // add the agents 
            DifferentialDriveAgent driveFeedback = NavigationManifest.AddDifferentialDrive(manifest, views, heartbeat, robotConfigFolder);
            VirtualDriveFeedbackAgent virtualDrive = NavigationManifest.AddVirtualDriveFeedback(manifest, views, driveFeedback, heartbeat, robotConfigFolder);
            SLAM2DAgent<KinectHorizontalDepthProfile, DriveFeedbackState> slam = NavigationManifest.AddSLAM2D(manifest, views, hdp, virtualDrive, null, startPose, robotConfigFolder, DepthImageConstants.MaximumRange);

            // add the UI
                UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
                manifest.Agents.Add(userInterface);

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(slam.Name);
            manifest.ActiveAgents.Add(userInterface.Name);

            return manifest;
        }

        /// <summary>
        /// Generates a manifest that runs the SLAM metrics calculation offline using both wheel and turret motion.
        /// </summary>
        /// <param name="recordingsDirectory">The directory that contains the recorded files</param>
        /// <param name="groundTruthFile">The ground truth file matching the recording</param>
        /// <param name="iterationCount">The number of times to run the recording</param>
        /// <param name="startPose">The start pose of the robot, relative to the bottom right corner of the map, in map units.</param>
        /// <param name="mapHeight">Map height</param>
        /// <param name="mapWidth">Map width</param>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <param name="addUI">If true, the UI agent is added to the pipeline. Needed for automated metrics runs</param>
        /// <param name="particleCount">Number of slam particles. Default is 100.</param>
        /// <returns>The SLAM metrics manifest</returns>
        public static Manifest CreateSlamMetricsManifest(
            string recordingsDirectory,
            string groundTruthFile,
            int iterationCount,
            Pose2D startPose,
            int mapHeight,
            int mapWidth,
            string robotConfigFolder,
            bool addUI = true,
            int particleCount = 50)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the file players
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views, 1, 20);
            FilePlayerProxy gamepad = InteractionManifest.AddGamePadPlayback(manifest, views, timer, recordingsDirectory);

            DepthImageDecompressionAgent<KinectDepthCameraParameters> kinectDepth = VisionManifest.AddDepthPlayback<KinectDepthCameraParameters>(
               manifest,
               timer,
               recordingsDirectory,
               "KinectCompressedDepthAgent.xml");

            FilePlayerProxy heartbeat;
            KinectAgentMessageProcessor processor = VisionManifest.AddKinectProcessor(manifest, views, kinectDepth, robotConfigFolder);

            heartbeat = EV4Manifest.AddEV4HeartbeatPlayback(manifest, views, timer, recordingsDirectory);
            JointMessageInterpolatorAgent jointMessageInterpolator = NavigationManifest.AddJointMessageInterpolator(manifest, views, heartbeat.AsProducer<JointMessage>(), processor);
            GyroscopeMessageInterpolatorAgent gyroscopeMessageInterpolator = NavigationManifest.AddGyroscopeMessageInterpolator(manifest, views, heartbeat.AsProducer<GyroscopeMessage>(), processor);
            DifferentialDriveMessageInterpolatorAgent differentialDriveMessageInterpolator = NavigationManifest.AddDifferentialDriveMessageInterpolator(manifest, views, heartbeat.AsProducer<DifferentialDriveMessage>(), processor);

            KinectPoseUpdaterAgent kinectPose = AddKinectPoseUpdater(manifest, views, processor, jointMessageInterpolator);

            DifferentialDriveAgent driveFeedback = NavigationManifest.AddDifferentialDrive(manifest, views, differentialDriveMessageInterpolator, robotConfigFolder);

            VirtualDriveFeedbackAgent virtualDrive = NavigationManifest.AddVirtualDriveFeedback(manifest, views, driveFeedback, gyroscopeMessageInterpolator, robotConfigFolder);

            KinectHorizontalDepthProfileAgent hdp = NavigationManifest.AddKinectHorizontalDepthProfile(manifest, views, kinectPose, robotConfigFolder);

            SLAM2DAgent<KinectHorizontalDepthProfile, DriveFeedbackState> slam = NavigationManifest.AddSLAM2D(manifest, views, hdp, virtualDrive, null, startPose, robotConfigFolder, DepthImageConstants.MaximumRange, mapHeight, mapWidth);

            GraphOptimizationPose2DAgent<KinectHorizontalDepthProfile> graphOptimizationAgent = NavigationManifest.Add2DGraphOptimizer(
                manifest, 
                views, 
                recordingsDirectory, 
                mapWidth, 
                mapHeight, 
                startPose, 
                slam, 
                gamepad.AsProducer<GamePadAgentMessage>(), 
                gamepad.AsProducer<CompletionMessage>(), 
                false);

            // Get the metrics
            GroundTruthFromStopAndGoFileAgent groundTruth = new GroundTruthFromStopAndGoFileAgent(
                name: "GroundTruthAgent",
                producer: gamepad.AsProducer<GamePadAgentMessage>(),
                groundTruthFile: Path.Combine(recordingsDirectory, "GroundTruth.txt"));
            manifest.Agents.Add(groundTruth);

            SLAM2DToPose2DTransformerAgent slam2DToPose2D = NavigationManifest.AddSlam2DToPose2DTransformerAgent(manifest, views, graphOptimizationAgent);
            PositioningPrecisionCalculator calc = NavigationManifest.AddPositioningPrecisionCalculator(manifest, views, slam2DToPose2D, groundTruth, graphOptimizationAgent, 10000);

            MetricsController controller = new MetricsController(
                name: "controller",
                calculatorAgent: calc,
                completionAgents: new IProducer<CompletionMessage>[0], // no completion agents.  Calculator will indicate completion via metrics message.
                iterations: iterationCount);
            manifest.Agents.Add(controller);
            manifest.ActiveAgents.Add(controller.Name);

            ////// add the UI
                UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
                manifest.Agents.Add(userInterface);

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(userInterface.Name);

            return manifest;
        }

        /// <summary>
        /// Creates a manifest that can be used to post-process a recording to produce a new set of recording containing only data when turret is rotating.
        /// </summary>
        /// <returns>A manifest for converting a full recording to a recording containing only rotation data.</returns>
        /// <param name="recordingsDirectory">Recordings directory</param>
        /// <param name="outputDirectory">The directory to output the transformed recordings</param>
        /// <param name="groundTruthFile">Ground truth file for perfect odometry</param>
        /// <param name="robotConfigFolder">Robot config folder</param>
        /// <param name="addUI">Determines whether to add user interface agent</param>
        public static Manifest CreateSlamRecordingTransformerManifest(
            string recordingsDirectory,
            string outputDirectory,
            string groundTruthFile,
            string robotConfigFolder,
            bool addUI = true)
        {
            Manifest manifest = new Manifest();

            List<ViewDefinition> views = new List<ViewDefinition>();

            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views, 1, 20);

            FilePlayerProxy gamepad = InteractionManifest.AddGamePadPlayback(manifest, null, timer, recordingsDirectory);

            DepthImageDecompressionAgent<KinectDepthCameraParameters> kinectDepth = VisionManifest.AddDepthPlayback<KinectDepthCameraParameters>(
               manifest,
               timer,
               recordingsDirectory,
               "KinectCompressedDepthAgent.xml");

            FilePlayerProxy heartbeat = EV4Manifest.AddEV4HeartbeatPlayback(manifest, null, timer, recordingsDirectory);
            FilePlayerProxy rgb = VisionManifest.AddRGBCameraPlayback(manifest, views, timer, recordingsDirectory);

            DifferentialDriveAgent driveFeedback = NavigationManifest.AddDifferentialDrive(manifest, views, heartbeat.AsProducer<DifferentialDriveMessage>(), robotConfigFolder);

            VirtualDriveFeedbackAgent virtualDrive = NavigationManifest.AddVirtualDriveFeedback(manifest, views, driveFeedback, heartbeat.AsProducer<GyroscopeMessage>(), robotConfigFolder);

            SlamRecordingTransformerAgent<DriveFeedbackState> transformer = new SlamRecordingTransformerAgent<DriveFeedbackState>(
                "SlamRecordingTransformerAgent",
                groundTruthFile,
                kinectDepth,
                gamepad.AsProducer<GamePadAgentMessage>(),
                heartbeat.AsProducer<AccelerometerMessage>(),
                heartbeat.AsProducer<CompassMessage>(),
                heartbeat.AsProducer<GyroscopeMessage>(),
                heartbeat.AsProducer<SonarMessage>(),
                heartbeat.AsProducer<IRProximityMessage>(),
                heartbeat.AsProducer<DifferentialDriveMessage>(),
                heartbeat.AsProducer<BatteryMessage>(),
                heartbeat.AsProducer<JointMessage>(),
                virtualDrive);

            FileRecorderProxy kinectRecorder = new FileRecorderProxy(
            name: transformer.Name + "Kinect",
            filePath: outputDirectory,
            messageTypes: new[] { typeof(DepthCameraAgentMessage<KinectDepthCameraParameters>) },
            producer: transformer.Name,
            enableArrayCompression: true);

            FileRecorderProxy gamepadRecorder = new FileRecorderProxy(
            name: transformer.Name + "Gamepad",
            filePath: outputDirectory,
            messageTypes: new[] { typeof(GamePadAgentMessage) },
            producer: transformer.Name,
            enableArrayCompression: true);

            FileRecorderProxy heartbeatRecorder = new FileRecorderProxy(
              name: transformer.Name + "Heartbeat",
              filePath: outputDirectory,
              messageTypes: new[] 
            { 
                typeof(AccelerometerMessage),
                typeof(CompassMessage),
                typeof(GyroscopeMessage),
                typeof(SonarMessage),
                typeof(IRProximityMessage),
                typeof(DifferentialDriveMessage),
                typeof(BatteryMessage),
                typeof(JointMessage)
            },
            producer: transformer.Name,
            enableArrayCompression: true);

            FileRecorderProxy driveFeedbackRecorder = new FileRecorderProxy(
             name: transformer.Name + "DriveFeedback",
             filePath: outputDirectory,
             messageTypes: new[] 
            { 
                typeof(SimpleAgentMessage<DriveFeedbackState>)
            },
           producer: transformer.Name,
           enableArrayCompression: true);

            manifest.Agents.Add(transformer);
            manifest.Agents.Add(heartbeatRecorder);
            manifest.Agents.Add(driveFeedbackRecorder);
            manifest.Agents.Add(gamepadRecorder);
            manifest.Agents.Add(kinectRecorder);
            manifest.ActiveAgents.Add(transformer.Name);

            // add the UI
            if (addUI)
            {
                UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
                manifest.Agents.Add(userInterface);
                manifest.ActiveAgents.Add(userInterface.Name);
            }

            manifest.ActiveAgents.Add(heartbeatRecorder.Name);
            manifest.ActiveAgents.Add(driveFeedbackRecorder.Name);
            manifest.ActiveAgents.Add(gamepadRecorder.Name);
            manifest.ActiveAgents.Add(kinectRecorder.Name);

            return manifest;
        }

        /// <summary>
        /// Generates a manifest that runs the SLAM metrics calculation offline using only turret motion and wheel odometry from an existing recording.
        /// </summary>
        /// <param name="recordingsDirectory">The directory that contains the recorded files</param>
        /// <param name="groundTruthFile">The ground truth file matching the recording</param>
        /// <param name="iterationCount">The number of times to run the recording</param>
        /// <param name="startPose">The start pose of the robot, relative to the bottom right corner of the map, in map units.</param>
        /// <param name="mapHeight">Map height</param>
        /// <param name="mapWidth">Map width</param>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <param name="addUI">If true, the UI agent is added to the pipeline. Needed for automated metrics runs</param>
        /// <param name="particleCount">Slam particle count</param>
        /// <returns>The SLAM metrics manifest</returns>
        public static Manifest CreateSlamRecordingTransformerDirectPlaybackManifest(
            string recordingsDirectory,
            string groundTruthFile,
            int iterationCount,
            Pose2D startPose,
            int mapHeight,
            int mapWidth,
            string robotConfigFolder,
            bool addUI = true,
            int particleCount = 100)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the file players
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views, 1, 1);
            FilePlayerProxy gamepad = InteractionManifest.AddGamePadPlayback(manifest, views, timer, recordingsDirectory);

            DepthImageDecompressionAgent<KinectDepthCameraParameters> kinectDepth = VisionManifest.AddDepthPlayback<KinectDepthCameraParameters>(
               manifest,
               timer,
               recordingsDirectory,
               "KinectCompressedDepthAgent.xml");

            FilePlayerProxy heartbeat;
            heartbeat = EV4Manifest.AddEV4HeartbeatPlayback(manifest, views, timer, recordingsDirectory);

            DifferentialDriveAgent driveFeedback = NavigationManifest.AddDifferentialDrive(manifest, views, heartbeat.AsProducer<DifferentialDriveMessage>(), robotConfigFolder);

            VirtualDriveFeedbackAgent virtualDrive = NavigationManifest.AddVirtualDriveFeedback(manifest, views, driveFeedback, heartbeat.AsProducer<GyroscopeMessage>(), robotConfigFolder);

            SlamRecordingTransformerAgent<DriveFeedbackState> transformer = new SlamRecordingTransformerAgent<DriveFeedbackState>(
                    "TransformerAgent",
                    groundTruthFile,
                    kinectDepth,
                    gamepad.AsProducer<GamePadAgentMessage>(),
                    heartbeat.AsProducer<AccelerometerMessage>(),
                    heartbeat.AsProducer<CompassMessage>(),
                    heartbeat.AsProducer<GyroscopeMessage>(),
                    heartbeat.AsProducer<SonarMessage>(),
                    heartbeat.AsProducer<IRProximityMessage>(),
                    heartbeat.AsProducer<DifferentialDriveMessage>(),
                    heartbeat.AsProducer<BatteryMessage>(),
                    heartbeat.AsProducer<JointMessage>(),
                    virtualDrive);

            manifest.Agents.Add(transformer);
            KinectAgentMessageProcessor processor = VisionManifest.AddKinectProcessor(manifest, views, transformer, robotConfigFolder);
            KinectPoseUpdaterAgent kinectPose = NavigationManifest.AddKinectPoseUpdater(manifest, views, processor, heartbeat.AsProducer<JointMessage>());
            KinectHorizontalDepthProfileAgent hdp = NavigationManifest.AddKinectHorizontalDepthProfile(manifest, views, kinectPose, robotConfigFolder);

            SLAM2DAgent<KinectHorizontalDepthProfile, DriveFeedbackState> slam = NavigationManifest.AddSLAM2D<KinectHorizontalDepthProfile, DriveFeedbackState>(manifest, views, hdp, transformer, null, startPose, robotConfigFolder, DepthImageConstants.MaximumRange, mapHeight, mapWidth, particleCount);

            // add the metrics 
            GroundTruthFromStopAndGoFileAgent groundTruth = new GroundTruthFromStopAndGoFileAgent(
                name: "GroundTruthAgent",
                producer: transformer,
                groundTruthFile: groundTruthFile);
            manifest.Agents.Add(groundTruth);
                        
            SLAM2DToPose2DTransformerAgent slam2DToPose2D = NavigationManifest.AddSlam2DToPose2DTransformerAgent(manifest, views, slam);
            PositioningPrecisionCalculator calc = NavigationManifest.AddPositioningPrecisionCalculator(manifest, views, slam2DToPose2D, groundTruth, gamepad, 10000);
            
            MetricsController controller = new MetricsController(
                name: "controller",
                calculatorAgent: calc,
                completionAgents: new IProducer<CompletionMessage>[0], // no completion agents.  Calculator will indicate completion via metrics message.
                iterations: iterationCount);
            manifest.Agents.Add(controller);

            // add the UI
            if (addUI)
            {
                UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
                manifest.Agents.Add(userInterface);
                manifest.ActiveAgents.Add(userInterface.Name);
            }

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(controller.Name);

            return manifest;
        }

        /// <summary>
        /// Generates a manifest that runs the SLAM metrics calculation offline using only turret motion and wheel odometry from a recording which has been 
        /// transformed using SlamRecordingTransformer.
        /// </summary>
        /// <param name="recordingsDirectory">The directory that contains the recorded files</param>
        /// <param name="groundTruthFile">The ground truth file matching the recording</param>
        /// <param name="iterationCount">The number of times to run the recording</param>
        /// <param name="startPose">The start pose of the robot, relative to the bottom right corner of the map, in map units.</param>
        /// <param name="mapHeight">Map height</param>
        /// <param name="mapWidth">Map width</param>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <param name="addUI">If true, the UI agent is added to the pipeline. Needed for automated metrics runs</param>
        /// <param name="particleCount">Slam particle count</param>
        /// <returns>The SLAM metrics manifest</returns>
        public static Manifest CreateSlamMetricsUnitTest(
            string recordingsDirectory,
            string groundTruthFile,
            int iterationCount,
            Pose2D startPose,
            int mapHeight,
            int mapWidth,
            string robotConfigFolder,
            bool addUI = true,
            int particleCount = 100)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the file players
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views, 1, 20);
            FilePlayerProxy gamepad = InteractionManifest.AddGamePadPlayback(manifest, views, timer, recordingsDirectory);

            DepthImageDecompressionAgent<KinectDepthCameraParameters> kinectDepth = VisionManifest.AddDepthPlayback<KinectDepthCameraParameters>(
               manifest,
               timer,
               recordingsDirectory,
               "KinectCompressedDepthAgent.xml");

            FilePlayerProxy heartbeat = EV4Manifest.AddEV4HeartbeatPlayback(manifest, views, timer, recordingsDirectory);
            FilePlayerProxy driveFeedback = EV4Manifest.AddDriveFeedbackPlayback(manifest, views, timer, recordingsDirectory);            
            VirtualDriveFeedbackAgent virtualDrive = NavigationManifest.AddVirtualDriveFeedback(
                manifest,
                views,
                driveFeedback.AsProducer<SimpleAgentMessage<DriveFeedbackState>>(),
                heartbeat.AsProducer<GyroscopeMessage>(),
                robotConfigFolder);

            KinectAgentMessageProcessor processor = VisionManifest.AddKinectProcessor(manifest, views, kinectDepth, robotConfigFolder);

            KinectPoseUpdaterAgent kinectPose = AddKinectPoseUpdater(manifest, views, processor, heartbeat.AsProducer<JointMessage>());
            KinectHorizontalDepthProfileAgent hdp = NavigationManifest.AddKinectHorizontalDepthProfile(manifest, views, kinectPose, robotConfigFolder);

            SLAM2DAgent<KinectHorizontalDepthProfile, DriveFeedbackState> slam = NavigationManifest.AddSLAM2D<KinectHorizontalDepthProfile, DriveFeedbackState>(manifest, views, hdp, driveFeedback.AsProducer<SimpleAgentMessage<DriveFeedbackState>>(), null, startPose, robotConfigFolder, DepthImageConstants.MaximumRange, mapHeight, mapWidth, particleCount);

            // add the metrics 
            GroundTruthFromStopAndGoFileAgent groundTruth = new GroundTruthFromStopAndGoFileAgent(
                name: "GroundTruthAgent",
                producer: gamepad.AsProducer<GamePadAgentMessage>(),
                groundTruthFile: groundTruthFile);
            manifest.Agents.Add(groundTruth);

            SLAM2DToPose2DTransformerAgent slam2DToPose2D = NavigationManifest.AddSlam2DToPose2DTransformerAgent(manifest, views, slam);
            PositioningPrecisionCalculator calc = NavigationManifest.AddPositioningPrecisionCalculator(manifest, views, slam2DToPose2D, groundTruth, gamepad, 10000);
            
            MetricsController controller = new MetricsController(
                name: "controller",
                calculatorAgent: calc,
                completionAgents: new IProducer<CompletionMessage>[0], // no completion agents.  Calculator will indicate completion via metrics message.
                iterations: iterationCount);
            manifest.Agents.Add(controller);

            // add the UI
            if (addUI)
            {
                UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
                manifest.Agents.Add(userInterface);
                manifest.ActiveAgents.Add(userInterface.Name);
            }

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(controller.Name);

            return manifest;
        }

        /// <summary>
        /// Generates a manifest that runs the Localization metrics calculation offline
        /// </summary>
        /// <param name="recordingsDirectory">The directory that contains the recorded files</param>
        /// <param name="mapFile">The map to load and localize on</param>
        /// <param name="groundTruthFile">The ground truth file matching the recording</param>
        /// <param name="iterationCount">The number of times to run the recording</param>
        /// <param name="startPose">The start pose of the robot, relative to the bottom right corner of the map, in map units.</param>
        /// <param name="startPoseVariance">The uncertainty in the initial pose estimate</param>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <param name="addUI">If true, the UI agent is added to the pipeline. Needed for automated metrics runs</param>
        /// <param name="particleCount">Count of particles used for localization.</param>
        /// <returns>The SLAM metrics manifest</returns>
        public static Manifest CreateLocalizationMetricsManifest(
            string recordingsDirectory,
            string mapFile,
            string groundTruthFile,
            int iterationCount,
            Pose2D startPose,
            Pose2D startPoseVariance,
            string robotConfigFolder,
            bool addUI = true,
            int particleCount = 200)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the file players
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views, 1, 1);
            FilePlayerProxy gamepad = InteractionManifest.AddGamePadPlayback(manifest, views, timer, recordingsDirectory);

            DepthImageDecompressionAgent<KinectDepthCameraParameters> kinectDepth = VisionManifest.AddDepthPlayback<KinectDepthCameraParameters>(
               manifest,
               timer,
               recordingsDirectory,
               "KinectCompressedDepthAgent.xml");

            FilePlayerProxy heartbeat = EV4Manifest.AddEV4HeartbeatPlayback(manifest, views, timer, recordingsDirectory);

            // add the agents 
            KinectAgentMessageProcessor processor = VisionManifest.AddKinectProcessor(manifest, views, kinectDepth, robotConfigFolder);
            JointMessageInterpolatorAgent jointMessageInterpolator = NavigationManifest.AddJointMessageInterpolator(manifest, views, heartbeat.AsProducer<JointMessage>(), processor);
            GyroscopeMessageInterpolatorAgent gyroscopeMessageInterpolator = NavigationManifest.AddGyroscopeMessageInterpolator(manifest, views, heartbeat.AsProducer<GyroscopeMessage>(), processor);
            DifferentialDriveMessageInterpolatorAgent differentialDriveMessageInterpolator = NavigationManifest.AddDifferentialDriveMessageInterpolator(manifest, views, heartbeat.AsProducer<DifferentialDriveMessage>(), processor);

            KinectPoseUpdaterAgent kinectPose = AddKinectPoseUpdater(manifest, views, processor, jointMessageInterpolator);

            DifferentialDriveAgent driveFeedback = NavigationManifest.AddDifferentialDrive(manifest, views, differentialDriveMessageInterpolator, robotConfigFolder);

            VirtualDriveFeedbackAgent virtualDrive = NavigationManifest.AddVirtualDriveFeedback(manifest, views, driveFeedback, gyroscopeMessageInterpolator, robotConfigFolder);
            KinectHorizontalDepthProfileAgent hdp = NavigationManifest.AddKinectHorizontalDepthProfile(manifest, views, kinectPose, robotConfigFolder);

            StaticMapLocalizationAgent<DriveFeedbackState> loc = NavigationManifest.AddStaticMapLocalization<DriveFeedbackState>(
            manifest,
            views,
            hdp,
            driveFeedback,
            mapFile,
            startPose,
            startPoseVariance,
            robotConfigFolder,
            particleCount);

            // add the metrics 
            GroundTruthFromStopAndGoFileAgent groundTruth = new GroundTruthFromStopAndGoFileAgent(
                name: "GroundTruthAgent",
                producer: gamepad.AsProducer<GamePadAgentMessage>(),
                groundTruthFile: groundTruthFile);
            manifest.Agents.Add(groundTruth);

            SLAM2DToPose2DTransformerAgent slam2DToPose2D = NavigationManifest.AddSlam2DToPose2DTransformerAgent(manifest, views, loc);
            PositioningPrecisionCalculator calc = NavigationManifest.AddPositioningPrecisionCalculator(manifest, views, slam2DToPose2D, groundTruth, gamepad);

            MetricsController controller = new MetricsController(
                name: "controller",
                calculatorAgent: calc,
                completionAgents: new IProducer<CompletionMessage>[0], // no completion agents.  Calculator will indicate completion via metrics message.
                iterations: iterationCount);
            manifest.Agents.Add(controller);

            // add the UI
            if (addUI)
            {
                UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
                manifest.Agents.Add(userInterface);
                manifest.ActiveAgents.Add(userInterface.Name);
            }

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(controller.Name);

            return manifest;
        }

        /// <summary>
        /// Generates a manifest that runs the Localization on a recorded path. Note that this could result in different path
        /// from the actual traversed path. Use PlaybackFromPathTrackingRecording manifest to see the actual traversed path.
        /// </summary>
        /// <param name="recordingsDirectory">The directory that contains the recorded files</param>
        /// <param name="mapFile">The map to load and localize on</param>
        /// <param name="startPose">The start pose of the robot, relative to the bottom right corner of the map, in map units.</param>
        /// <param name="startPoseVariance">The uncertainty in the initial pose estimate</param>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <param name="addUI">If true, the UI agent is added to the pipeline. Needed for automated metrics runs</param>
        /// <param name="particleCount">Count of particles used for localization.</param>
        /// <param name="legacyWheelRotationRecording">True if legacy wheel recording which doesn't have pan information</param>
        /// <returns>The localization from recorded path manifest</returns>
        public static Manifest CreateLocalizationFromPathTrackingRecording(
            string recordingsDirectory,
            string mapFile,
            Pose2D startPose,
            Pose2D startPoseVariance,
            string robotConfigFolder,
            bool addUI = true,
            int particleCount = 200,
            bool legacyWheelRotationRecording = false)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the file players
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views, 1, 1);
            FilePlayerProxy hdp = new FilePlayerProxy("HdpPlayer", Path.Combine(recordingsDirectory, "HorizontalDepthProfileAgent.xml"), new[] { typeof(SimpleAgentMessage<KinectHorizontalDepthProfile>) }, timer);
            manifest.Agents.Add(hdp);
            FilePlayerProxy driveFeedback = EV4Manifest.AddDriveFeedbackPlayback(manifest, views, timer, recordingsDirectory);

            // add the agents 
            StaticMapLocalizationAgent<DriveFeedbackState> loc = NavigationManifest.AddStaticMapLocalization<DriveFeedbackState>(
                manifest,
                views,
                hdp.AsProducer<SimpleAgentMessage<KinectHorizontalDepthProfile>>(),
                driveFeedback.AsProducer<SimpleAgentMessage<DriveFeedbackState>>(),
                mapFile,
                startPose,
                startPoseVariance,
                robotConfigFolder,
                particleCount);

            FileRecorderProxy slamRecorder = NavigationManifest.AddSlamRecorder(manifest, views, loc);

            // add the UI
            if (addUI)
            {
                UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
                manifest.Agents.Add(userInterface);
                manifest.ActiveAgents.Add(userInterface.Name);
            }

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(loc.Name);
            manifest.ActiveAgents.Add(slamRecorder.Name);

            return manifest;
        }

        /// <summary>
        /// Generates a manifest that runs plays back the recorded robot poses from a recording.
        /// </summary>
        /// <param name="recordingsDirectory">The directory that contains the recorded files</param>
        /// <param name="mapFile">The map to load and localize on</param>
        /// <param name="startPose">The start pose of the robot, relative to the bottom right corner of the map, in map units.</param>
        /// <param name="startPoseVariance">The uncertainty in the initial pose estimate</param>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <param name="addUI">If true, the UI agent is added to the pipeline. Needed for automated metrics runs</param>
        /// <param name="particleCount">Count of particles used for localization.</param>
        /// <param name="legacyWheelRotationRecording">True if legacy wheel recording which doesn't have pan information</param>
        /// <returns>The playback from path recording manifest</returns>
        public static Manifest CreatePlaybackFromPathTrackingRecording(
            string recordingsDirectory,
            string mapFile,
            Pose2D startPose,
            Pose2D startPoseVariance,
            string robotConfigFolder,
            bool addUI = true,
            int particleCount = 200,
            bool legacyWheelRotationRecording = false)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the file players
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views, 1, 1);

            DepthImageDecompressionAgent<KinectDepthCameraParameters> kinectDepth = VisionManifest.AddDepthPlayback<KinectDepthCameraParameters>(
               manifest,
               timer,
               recordingsDirectory,
               "KinectCompressedDepthAgent.xml");

            // add the agents 
            KinectAgentMessageProcessor processor = VisionManifest.AddKinectProcessor(manifest, views, kinectDepth, robotConfigFolder);

            KinectHorizontalDepthProfileAgent hdp = NavigationManifest.AddKinectHorizontalDepthProfile(manifest, views, processor, robotConfigFolder);
            FilePlayerProxy slamAgent = new FilePlayerProxy("SlamAgent", Path.Combine(recordingsDirectory, "SlamAgent.xml"), new[] { typeof(SLAM2DAgentMessage) }, timer);
            manifest.Agents.Add(slamAgent);
            NavigationManifest.AddGlobalMapModelView(manifest, views, slamAgent.AsProducer<SLAM2DAgentMessage>(), hdp, DepthImageConstants.MaximumRange, mapFile);

            // add the UI
            if (addUI)
            {
                UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
                manifest.Agents.Add(userInterface);
                manifest.ActiveAgents.Add(userInterface.Name);                
            }

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(slamAgent.Name);
            manifest.ActiveAgents.Add(kinectDepth.Name);
            manifest.ActiveAgents.Add(hdp.Name);

            return manifest;
        }

        /// <summary>
        /// Adds a slam recorder to the manifest.
        /// </summary>
        /// <param name="manifest">Manifest to add the slam recorder to</param>
        /// <param name="views">The list of views</param>
        /// <param name="slamProducer">Slam agent message producer</param>
        /// <returns>Filer recorder for slam agent messages</returns>
        public static FileRecorderProxy AddSlamRecorder(Manifest manifest, List<ViewDefinition> views, IProducer<SLAM2DAgentMessage> slamProducer)
        {
            FileRecorderProxy slamRecorder = new FileRecorderProxy(
               name: slamProducer.Name + "Recorder",
               filePath: null,
               messageTypes: new[] { typeof(SLAM2DAgentMessage) },
               producer: slamProducer.Name);
            manifest.Agents.Add(slamRecorder);

            return slamRecorder;
        }

        /// <summary>
        /// Generates a manifest that runs the SLAM metrics calculation offline
        /// </summary>
        /// <param name="recording">The recorded sensor stream file</param>
        /// <param name="groundTruthFile">The ground truth file matching the recording</param>
        /// <param name="iterationCount">The number of times to run the recording</param>
        /// <param name="robotStartPose">The start pose of the robot, relative to the bottom right corner of the map, in map units.</param>
        /// <param name="mapHeight">Map height in map units</param>
        /// <param name="mapWidth">Map width in map units</param>
        /// <param name="robotConfigFolder">Robot calibration folder.</param>
        /// <param name="addUI">If true, the UI agent is added to the pipeline. Needed for automated metrics runs</param>
        /// <param name="particleCount">Number of particles</param>
        /// <returns>The SLAM metrics manifest</returns>
        public static Manifest CreateExternalDatasetSLAMMetricsManifest(string recording, string groundTruthFile, int iterationCount, Pose2D robotStartPose, int mapHeight, int mapWidth, string robotConfigFolder, bool addUI = true, int particleCount = 50)
        {
            const double DistanceResolution = 0.1;
            const double DepthCutoff = 80;
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the file players
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views, 1, 20);
            FilePlayerAgent sensors = new ExternalDatasetSensorAgent(
                name: "sensors",
                recording: recording,
                timerAgent: timer,
                robotConfigFolder: robotConfigFolder);
            manifest.Agents.Add(sensors);

            IProducer<SimpleAgentMessage<LaserHorizontalDepthProfile>> hdp = sensors.AsProducer<SimpleAgentMessage<LaserHorizontalDepthProfile>>();

            PoseFeedbackMessageInterpolatorAgent poseFeedbackInterpolator = NavigationManifest.AddPoseFeedbackMessageInterpolator(manifest, views, sensors.AsProducer<SimpleAgentMessage<PoseFeedbackState>>(), hdp);

            NavigationManifest.AddHorizontalDepthProfileModelView(
                manifest,
                views,
                hdp,
                DistanceResolution,
                DepthCutoff);

            SLAM2DAgent<LaserHorizontalDepthProfile, PoseFeedbackState> slam = new SLAM2DAgent<LaserHorizontalDepthProfile, PoseFeedbackState>(
                name: "SLAM2DAgent",
                hdpProducer: hdp,
                driveFeedbackProducer: sensors.AsProducer<SimpleAgentMessage<PoseFeedbackState>>(),
                robotConfigFolder: robotConfigFolder,
                distanceResolution: 0.1f,
                particleCount: particleCount,
                mapHeight: mapHeight,
                mapWidth: mapWidth,
                mapFile: null,
                robotStartPose: robotStartPose);
            manifest.Agents.Add(slam);

            // add the model and view
            NavigationManifest.AddGlobalMapModelView<LaserHorizontalDepthProfile>(manifest, views, slam, hdp, DepthCutoff, null);

            // add the metrics 
            FilePlayerAgent groundTruth = new ExternalDatasetGroundTruthAgent(
                name: "GroundTruthAgent",
                groundTruthFile: groundTruthFile,
                timerAgent: timer);
            manifest.Agents.Add(groundTruth);

            SLAM2DToPose2DTransformerAgent slam2DToPose2D = NavigationManifest.AddSlam2DToPose2DTransformerAgent(manifest, views, slam);
            PositioningPrecisionCalculator calc = NavigationManifest.AddPositioningPrecisionCalculator(manifest, views, slam2DToPose2D, groundTruth.AsProducer<GroundTruthMessage>(), groundTruth.AsProducer<CompletionMessage>());

            MetricsController controller = new MetricsController(
                name: "controller",
                calculatorAgent: calc,
                completionAgents: new IProducer<CompletionMessage>[0], // no completion agents.  Calculator will indicate completion via metrics message.
                iterations: iterationCount);
            manifest.Agents.Add(controller);

            // add the UI
            if (addUI)
            {
                UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
                manifest.Agents.Add(userInterface);
                manifest.ActiveAgents.Add(userInterface.Name);
            }

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(controller.Name);

            return manifest;
        }

        /// <summary>
        /// Generates a manifest that runs the SLAM metrics calculation on results published for some other SLAM implementation. 
        /// Used to establish the metrics goal.
        /// </summary>
        /// <param name="resultFile">The result file from a 3rd party slam implementation</param>
        /// <param name="groundTruthFile">The ground truth file matching the dataset used to generate the slam results</param>
        /// <param name="startPose">The start pose of the robot, relative to the bottom right corner of the map, in map units.</param>
        /// <returns>The playback manifest</returns>
        public static Manifest CreateExternalDataset3rdPartySLAMResultsManifest(string resultFile, string groundTruthFile, Pose2D startPose)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the slam player
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views);
            FilePlayerAgent slam = new ExternalDatasetSLAMOutputAgent(
                name: "sensors",
                resultFile: resultFile,
                timerAgent: timer);
            manifest.Agents.Add(slam);

            // add the model and view
            NavigationManifest.AddSimpleGlobalMapModelView(manifest, views, slam.AsProducer<SLAM2DAgentMessage>());

            // add the metrics 
            FilePlayerAgent groundTruth = new ExternalDatasetGroundTruthAgent(
                name: "GroundTruthAgent",
                groundTruthFile: groundTruthFile,
                timerAgent: timer);
            manifest.Agents.Add(groundTruth);

            SLAM2DToPose2DTransformerAgent slam2DToPose2D = NavigationManifest.AddSlam2DToPose2DTransformerAgent(manifest, views, slam.AsProducer<SLAM2DAgentMessage>());
            PositioningPrecisionCalculator calc = NavigationManifest.AddPositioningPrecisionCalculator(manifest, views, slam2DToPose2D, groundTruth.AsProducer<GroundTruthMessage>(), groundTruth.AsProducer<CompletionMessage>());

            MetricsController controller = new MetricsController(
                name: "controller",
                calculatorAgent: calc,
                completionAgents: new IProducer<CompletionMessage>[0], // no completion agents.  Calculator will indicate completion via metrics message.
                iterations: 1);
            manifest.Agents.Add(controller);

            // add the UI
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(controller.Name);
            manifest.ActiveAgents.Add(userInterface.Name);

            return manifest;
        }

        /// <summary>
        /// Create a manifest to create ground truth files while driving robot manually from landmark to landmark
        /// </summary>
        /// <param name="robotCalibrationFolder">Calibration Folder</param>
        /// <param name="environmentConfigFolder">Environment config folder</param>
        /// <param name="waypointFile">Path to file of landmark Ids in the order they will be visited</param>
        /// <param name="outputDir">Path of the directory to which ground truth files will be written</param>
        /// <param name="record">If true, then gamepad and RBG messages will be recorded</param>
        /// <returns>A manifest that produces ground truth files</returns>
        public static Manifest CreateGroundTruthLoggerManifest(string robotCalibrationFolder, string environmentConfigFolder, string waypointFile, string outputDir, bool record)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // gamepad
            GamePadAgent gamepad = InteractionManifest.AddGamePadAgent(manifest, views);
            IProducer<RGBCameraAgentMessage> rgbCamera = VisionManifest.AddRGBCameraAgent(manifest, views, 1, 1);
            IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimation = VisionManifest.AddCameraPoseEstimationAgent(rgbCamera, manifest, views, robotCalibrationFolder, environmentConfigFolder);
            IProducer<RobotPoseEstimationAgentMessage> robotPoseEstimation = AddRobotPoseEstimationAgent(rgbCamera, cameraPoseEstimation, manifest, views, robotCalibrationFolder);
            GroundTruthLoggerAgent groundTruthLogger = AddGroundTruthLogger(robotCalibrationFolder, outputDir, waypointFile, manifest, views, gamepad, cameraPoseEstimation, robotPoseEstimation);

            // add the UI
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);
            manifest.ActiveAgents.Add(userInterface.Name);

            // specify the leaf agents to activate
            manifest.ActiveAgents.Add(userInterface.Name);
            manifest.ActiveAgents.Add(groundTruthLogger.Name);

            if (record)
            {
                // add the recorders
                FileRecorderProxy gamepadRecorder = InteractionManifest.AddGamePadRecorder(manifest, gamepad);
                FileRecorderProxy rgbCameraRecorder = VisionManifest.AddRGBCameraRecorder(manifest, rgbCamera);

                manifest.ActiveAgents.Add(gamepadRecorder.Name);
                manifest.ActiveAgents.Add(rgbCameraRecorder.Name);
            }

            return manifest;
        }

        /// <summary>
        /// Create a manifest to create ground truth files from recorded gamepad and RGB streams
        /// </summary>
        /// <param name="robotConfigFolder">Robot calibration Folder</param>
        /// <param name="environmentConfigFolder">Environment config folder</param>
        /// <param name="groundTruthOutputFolder">Path to the output folder for ground truth files</param>
        /// <param name="waypointFile">Path to file of landmark Ids in the order they will be visited</param>
        /// <param name="recordingsDirectory">Directory containing the recordings</param>
        /// <returns>A manifest that produces ground truth files</returns>
        public static Manifest CreateGroundTruthLoggerPlaybackManifest(string robotConfigFolder, string environmentConfigFolder, string groundTruthOutputFolder, string waypointFile, string recordingsDirectory)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // fileplayers
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views, 1, 5);
            FilePlayerProxy gamepad = InteractionManifest.AddGamePadPlayback(manifest, views, timer, recordingsDirectory);
            FilePlayerProxy rgbCamera = VisionManifest.AddRGBCameraPlayback(manifest, views, timer, recordingsDirectory);

            // working agents
            IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimation = VisionManifest.AddCameraPoseEstimationAgent(
                rgbCamera.AsProducer<RGBCameraAgentMessage>(),
                manifest,
                views,
                robotConfigFolder,
                environmentConfigFolder);

            IProducer<RobotPoseEstimationAgentMessage> robotPoseEstimationAgent = NavigationManifest.AddRobotPoseEstimationAgent(
                rgbCamera.AsProducer<RGBCameraAgentMessage>(),
                cameraPoseEstimation,
                manifest,
                views,
                robotConfigFolder);

            GroundTruthLoggerAgent groundTruthLogger = new GroundTruthLoggerAgent(
                name: "GroundTruthLogger",
                outputDir: groundTruthOutputFolder,
                gamePadProducer: gamepad.Name,
                cameraPoseProducer: cameraPoseEstimation.Name,
                robotPoseProducer: robotPoseEstimationAgent.Name,
                waypointFile: waypointFile);
            manifest.Agents.Add(groundTruthLogger);

            // add the UI
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);
            manifest.ActiveAgents.Add(userInterface.Name);

            // specify the leaf agents to activate            
            manifest.ActiveAgents.Add(gamepad.Name);
            manifest.ActiveAgents.Add(userInterface.Name);
            manifest.ActiveAgents.Add(robotPoseEstimationAgent.Name);
            manifest.ActiveAgents.Add(groundTruthLogger.Name);

            return manifest;
        }

        /// <summary>
        /// Generates a manifest that creates navigation waypoints using slam output, gamepad and the groundTruthLandmarkRelative file.
        /// </summary>
        /// <param name="recordingsDirectory">The directory that contains the recorded files</param>        
        /// <returns>The SLAM metrics manifest</returns>
        public static Manifest CreateNavigationWaypointsManifest(
            string recordingsDirectory)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the file players
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views, 1, 100);
            FilePlayerProxy gamepad = InteractionManifest.AddGamePadPlayback(manifest, views, timer, recordingsDirectory);

            FilePlayerProxy slamPlayer = new FilePlayerProxy(
                "SlamPlayer",
                Path.Combine(recordingsDirectory, "OptimizedTrajectory.xml"),
                new[] { typeof(SLAM2DAgentMessage) },
                timer);
            manifest.Agents.Add(slamPlayer);

            // add the agents 
            NavWaypointsFromSlamAgent waypointsFromLandmarkAgent = NavigationManifest.AddNavWaypointsFromSlamAgent(
                manifest,
                gamepad.AsProducer<GamePadAgentMessage>(),
                slamPlayer.AsProducer<SLAM2DAgentMessage>(),
                recordingsDirectory);

            manifest.ActiveAgents.Add(waypointsFromLandmarkAgent.Name);

            return manifest;
        }

        /// <summary>
        /// Create a manifest to publish metrics based on recorded RGB camera and a navigation data file
        /// </summary>
        /// <param name="robotConfigFolder">Robot calibration Folder</param>
        /// <param name="environmentConfigFolder">Environment config folder</param>
        /// <param name="recordingsDirectory">Directory containing the recordings</param>
        /// <param name="slamDirectory">Directory containing the SLAM data for the map used during navigation.</param>
        /// <param name="database">Indicates the run is local and will not be sent to database</param>
        /// <returns>A manifest that produces ground truth files</returns>
        public static Manifest CreateNavigationMetricsFromSavedDataManifest(string robotConfigFolder, string environmentConfigFolder, string recordingsDirectory, string slamDirectory, bool database)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // This can be played back accelerated as long as "/o" is on the commandline or the locator is initialized with offline flag true
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views, 1, 50);
            
            FilePlayerProxy rgbCamera = VisionManifest.AddRGBCameraPlayback(
                manifest: manifest, 
                views: views,
                timer: timer,
                directory: recordingsDirectory,
                addView: true);

            IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimation = VisionManifest.AddCameraPoseEstimationAgent(
                rgbCamera.AsProducer<RGBCameraAgentMessage>(),
                manifest,
                views,
                robotConfigFolder,
                environmentConfigFolder);

            IProducer<RobotPoseEstimationAgentMessage> robotPoseEstimationAgent = NavigationManifest.AddRobotPoseEstimationAgent(
                rgbCamera.AsProducer<RGBCameraAgentMessage>(),
                cameraPoseEstimation,
                manifest,
                views,
                robotConfigFolder);

            NavDataFilePlayerAgent navData = new NavDataFilePlayerAgent(
                name: "NavData",
                navDataFilePath: Path.Combine(recordingsDirectory, "NavData.txt"),
                timerAgent: timer,
                cameraPoseProducerName: cameraPoseEstimation.Name);
            manifest.Agents.Add(navData);

            NavigationMetricsAgent navMetricsAgent = new NavigationMetricsAgent(
                name: "NavigationMetrics",
                navDataProducer: navData.Name,
                robotPoseProducer: robotPoseEstimationAgent.Name,
                cameraPoseProducer: cameraPoseEstimation.Name,
                distancesFilePath: Path.Combine(slamDirectory, "Distances.xml"),
                groundTruthFilePath: Path.Combine(slamDirectory, "GroundTruthAbsolute.txt"),
                database: database);
            manifest.Agents.Add(navMetricsAgent);

            MetricsController controller = new MetricsController(
                name: "controller",
                calculatorAgent: navMetricsAgent,
                completionAgents: new[] { rgbCamera },
                iterations: 1);
            manifest.Agents.Add(controller);

            // add the UI
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);

            // specify the leaf agents to activate  
            manifest.ActiveAgents.Add(robotPoseEstimationAgent.Name);
            manifest.ActiveAgents.Add(controller.Name);
            manifest.ActiveAgents.Add(userInterface.Name);

            return manifest;
        }

        /// <summary>
        /// Generates a manifest for running small recordings on a map and computes Localization results
        /// Used in ShortNavigationTests class to run tests against different recordings with different start/end poses.
        /// </summary>
        /// <param name="recordingsDirectory">The directory that contains the recorded files</param>
        /// <param name="mapFile">The map to load and localize on</param>
        /// <param name="targetLandmarkID">Landmark ID to which we are driving</param>
        /// <param name="startPoseInMetersAndRads">The start pose of the robot, relative to the bottom right corner of the map, in map units.</param>
        /// <param name="startPoseVarianceInMetersAndRads">The uncertainty in the initial pose estimate</param>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <param name="environmentConfigFolder">Environment config folder, where LandmarksInfo.xml for a map we use lives</param>
        /// <param name="particleCount">Count of particles used for localization.</param>
        /// <param name="manuallySetFinishPose">Optional externally passed finish pose used when no ground truth markers are installed at the finish</param>
        /// <returns>The short run metrics manifest and verification agent tuple</returns>
        public static Tuple<Manifest, ShortNavRunVerifierAgent> CreateLocalizationMiniRunTestManifest(
            string recordingsDirectory,
            string mapFile,
            int targetLandmarkID,
            Pose2D startPoseInMetersAndRads,
            Pose2D startPoseVarianceInMetersAndRads,
            string robotConfigFolder,
            string environmentConfigFolder,
            int particleCount,
            Pose2D? manuallySetFinishPose)
        {
            Manifest manifest = new Manifest();
            List<ViewDefinition> views = new List<ViewDefinition>();

            // add the file players
            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views, 1, 100); // try runing as fast as /o option will allow us

            DepthImageDecompressionAgent<KinectDepthCameraParameters> kinectDepth = VisionManifest.AddDepthPlayback<KinectDepthCameraParameters>(
               manifest,
               timer,
               recordingsDirectory,
               "KinectCompressedDepthAgent.xml");

            FilePlayerProxy heartbeat = EV4Manifest.AddEV4HeartbeatPlayback(manifest, views, timer, recordingsDirectory);
            
            FilePlayerProxy rgbCamera = VisionManifest.AddRGBCameraPlayback(manifest, views, timer, recordingsDirectory);

            // ground truth
            IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimation = VisionManifest.AddCameraPoseEstimationAgent(
                rgbCamera.AsProducer<RGBCameraAgentMessage>(),
                manifest,
                views,
                robotConfigFolder,
                environmentConfigFolder);

            IProducer<RobotPoseEstimationAgentMessage> robotPoseEstimationAgent = NavigationManifest.AddRobotPoseEstimationAgent(
                rgbCamera.AsProducer<RGBCameraAgentMessage>(),
                cameraPoseEstimation,
                manifest,
                views,
                robotConfigFolder);

            // create a pipeline that produces ground truth and localization messages for the purpose of feeds them into 
            // ShortNavRunVerifierAgent
            KinectAgentMessageProcessor processor = VisionManifest.AddKinectProcessor(manifest, views, kinectDepth, robotConfigFolder);

            JointMessageInterpolatorAgent jointMessageInterpolator = NavigationManifest.AddJointMessageInterpolator(manifest, views, heartbeat.AsProducer<JointMessage>(), processor);
            GyroscopeMessageInterpolatorAgent gyroscopeMessageInterpolator = NavigationManifest.AddGyroscopeMessageInterpolator(manifest, views, heartbeat.AsProducer<GyroscopeMessage>(), processor);
            DifferentialDriveMessageInterpolatorAgent differentialDriveMessageInterpolator = NavigationManifest.AddDifferentialDriveMessageInterpolator(manifest, views, heartbeat.AsProducer<DifferentialDriveMessage>(), processor);

            KinectPoseUpdaterAgent kinectOnATurrentAgent = AddKinectPoseUpdater(manifest, views, processor, jointMessageInterpolator);
            DifferentialDriveAgent driveFeedback = NavigationManifest.AddDifferentialDrive(manifest, views, differentialDriveMessageInterpolator, robotConfigFolder);                                                   
            VirtualDriveFeedbackAgent virtualDrive = NavigationManifest.AddVirtualDriveFeedback(manifest, views, driveFeedback, gyroscopeMessageInterpolator, robotConfigFolder, 100); // smaller number of gyro samples is OK for short runs

            KinectHorizontalDepthProfileAgent hdp = NavigationManifest.AddKinectHorizontalDepthProfile(manifest, views, kinectOnATurrentAgent, robotConfigFolder);

            // Convert from meters and degrees to map cells and radians (which are the units used internally by localization)
            GlobalMap map = Serializer.Load<GlobalMap>(mapFile);            
            Pose2D startPoseVarianceInMapCellsAndRads = new Pose2D(map.MetersToMapCells(startPoseVarianceInMetersAndRads.X), map.MetersToMapCells(startPoseVarianceInMetersAndRads.Y), startPoseVarianceInMetersAndRads.Heading);

            Pose2D startPoseInMapCellsAndRads = new Pose2D(map.MetersToMapCells(startPoseInMetersAndRads.X), map.MetersToMapCells(startPoseInMetersAndRads.Y), startPoseInMetersAndRads.Heading);
            StaticMapLocalizationAgent<DriveFeedbackState> localizationAgent = NavigationManifest.AddStaticMapLocalization(manifest, views, hdp, virtualDrive, mapFile, startPoseInMapCellsAndRads, startPoseVarianceInMapCellsAndRads, robotConfigFolder, particleCount);

            ShortNavRunVerifierAgent shortRunVerifierAgent = new ShortNavRunVerifierAgent(
                name: "ShortNavRunVerifierAgent",
                groundTruthPoseProducer: robotPoseEstimationAgent.Name,
                navigationPoseProducer: localizationAgent.Name,
                landmarkIDSinkAgentName: cameraPoseEstimation.Name,
                completionEventAgentName: rgbCamera.Name,
                targetLandmarkID: targetLandmarkID,
                manuallySetFinishPose: manuallySetFinishPose);

            manifest.Agents.Add(shortRunVerifierAgent);

            // add the UI
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);            

            manifest.ActiveAgents.Add(shortRunVerifierAgent.Name);            
            manifest.ActiveAgents.Add(userInterface.Name); 

            // We need both of those on the outside to run the manifest programmatically, as well as to inspect 
            // the results of a run
            return new Tuple<Manifest, ShortNavRunVerifierAgent>(manifest, shortRunVerifierAgent);
        }
    }
}
